842 lines
22 KiB
C++
842 lines
22 KiB
C++
/*
|
|
* $Id: collision.cpp,v 1.24 2002/07/21 15:03:12 msell Exp $
|
|
*
|
|
*
|
|
* $Log: collision.cpp,v $
|
|
* Revision 1.24 2002/07/21 15:03:12 msell
|
|
* Äänet disabloitu
|
|
*
|
|
* Revision 1.23 2002/07/19 18:59:46 msell
|
|
* Alkuhommaa ja säätöä
|
|
*
|
|
* Revision 1.22 2002/07/18 23:05:31 msell
|
|
* Partikkelit ja kakkospelaajan liike
|
|
*
|
|
* Revision 1.21 2002/07/17 22:45:53 msell
|
|
* Ääniä vähän
|
|
*
|
|
* Revision 1.20 2002/07/15 15:22:07 msell
|
|
* Parantelua
|
|
*
|
|
* Revision 1.19 2002/07/14 21:40:43 msell
|
|
* Conflictit pois, liikkumiset (hyppy, kävely, lyönti), uusi areena
|
|
*
|
|
* Revision 1.18 2002/07/10 17:13:44 msell
|
|
* Törmäystarkastelun parantelua
|
|
*
|
|
* Revision 1.17 2002/07/08 22:53:38 msell
|
|
* Säätöä
|
|
*
|
|
* Revision 1.16 2002/07/08 18:28:47 msell
|
|
* Törmäystä ja ukkoja
|
|
*
|
|
* Revision 1.15 2002/07/07 23:21:11 msell
|
|
* Pientä parantelua
|
|
*
|
|
* Revision 1.14 2002/07/07 23:05:22 msell
|
|
* Osien liimaaminen toisiinsa (kesken)
|
|
*
|
|
* Revision 1.13 2002/07/07 15:29:07 msell
|
|
* Törmäyksien parantelua
|
|
*
|
|
* Revision 1.12 2002/07/04 21:05:41 msell
|
|
* Se toimii!! =)
|
|
* Törmäystarkistukset siis
|
|
*
|
|
* Revision 1.11 2002/06/30 16:05:04 msell
|
|
* Törmäyksien parantelua, transformaatioita mukana
|
|
*
|
|
* Revision 1.10 2002/06/27 14:39:48 msell
|
|
* Toimiva maila :)
|
|
* Pyörivät kappaleet siis antaa liike-energiaa liikkuville kappaleille (ei toisin päin vielä)
|
|
*
|
|
* Revision 1.9 2002/06/27 00:08:04 msell
|
|
* Kimmotukset palloille myös pyöritettyihin mesheihin
|
|
*
|
|
* Revision 1.8 2002/06/23 20:12:19 msell
|
|
* Parempi törmäystarkistus palloista mesheihin
|
|
*
|
|
* Revision 1.7 2002/06/20 22:50:12 msell
|
|
* Meshit
|
|
*
|
|
* Revision 1.6 2002/06/17 12:42:46 msell
|
|
* Hieman parempi törmäysmallinnus taas
|
|
*
|
|
* Revision 1.5 2002/06/15 22:56:37 msell
|
|
* Säätöä
|
|
*
|
|
* Revision 1.4 2002/06/15 17:18:37 msell
|
|
* Toimiva törmäystarkastus kiinteille laatikoille
|
|
*
|
|
* Revision 1.3 2002/06/14 00:26:17 msell
|
|
* 100 kimpoilevaa palloa ja vähän vaimennusta
|
|
*
|
|
* Revision 1.2 2002/06/14 00:05:05 msell
|
|
* Törmäyssimulaatio kunnossa toivon mukaan
|
|
*
|
|
* Revision 1.1 2002/06/11 23:11:45 msell
|
|
* Törmäystarkistusta
|
|
*
|
|
*
|
|
*
|
|
* $Date: 2002/07/21 15:03:12 $
|
|
*
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
|
|
#include "object.h"
|
|
#include "vector.h"
|
|
#include "collision.h"
|
|
#include "mesh.h"
|
|
#include "3dutils.h"
|
|
#include "world.h"
|
|
#include "audio.h"
|
|
|
|
|
|
//objectlist *collisionlists[32];
|
|
unsigned int collisionlinks[32];
|
|
|
|
Contact *contacts;
|
|
int contactcount;
|
|
|
|
void initCollisions(void){
|
|
int i;
|
|
for (i = 0; i < 32; i++){
|
|
//collisionlists[i] = NULL;
|
|
collisionlinks[i] = 0;
|
|
}
|
|
}
|
|
|
|
/*void addCollisionObject(Object *object, int group){
|
|
objectlist *node = new objectlist;
|
|
node->object = object;
|
|
node->next = collisionlists[group];
|
|
collisionlists[group] = node;
|
|
}*/
|
|
|
|
void addCollisionLink(int source, int target){
|
|
collisionlinks[source] |= (1<<target);
|
|
if (source != target) collisionlinks[target] |= (1<<source);
|
|
}
|
|
|
|
void removeCollisionLink(int source, int target){
|
|
collisionlinks[source] &= ~(1<<target);
|
|
if (source != target) collisionlinks[target] &= ~(1<<source);
|
|
}
|
|
|
|
bool isCollisionLink(int source, int target){
|
|
if (collisionlinks[source] & (1<<target)) return true;
|
|
return false;
|
|
}
|
|
|
|
void addCollision(Object *source, Object *target,
|
|
float *normal, float *contactpoint){
|
|
if (contactcount == MAXCONTACTS){
|
|
printf("Too many contacts!\n");
|
|
return;
|
|
}
|
|
Contact *contact = &contacts[contactcount++];
|
|
contact->object1 = source;
|
|
contact->object2 = target;
|
|
vectorCopy(contact->normal, normal);
|
|
vectorCopy(contact->position, contactpoint);
|
|
}
|
|
|
|
#define KINETICFRICTION 0.4
|
|
|
|
bool handleCollision(Contact *contact){
|
|
Object *source = contact->object1;
|
|
Object *target = contact->object2;
|
|
float *normal = contact->normal;
|
|
float *contactpoint = contact->position;
|
|
|
|
float sourcevelocity[3], targetvelocity[3];
|
|
float sourcecontactpoint[3], targetcontactpoint[3];
|
|
|
|
vectorSub(sourcecontactpoint, contactpoint, source->position);
|
|
source->getVelocity(sourcevelocity, sourcecontactpoint);
|
|
|
|
if (target == NULL){
|
|
vectorSet(targetcontactpoint, 0, 0, 0);
|
|
vectorSet(targetvelocity, 0, 0, 0);
|
|
} else{
|
|
vectorSub(targetcontactpoint, contactpoint, target->position);
|
|
target->getVelocity(targetvelocity, targetcontactpoint);
|
|
}
|
|
|
|
float deltavelocity[3];
|
|
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
|
|
float dot = vectorDot(deltavelocity, normal);
|
|
|
|
//if (fabs(dot) < EPSILON) return false;
|
|
//if (dot > -1.0e-5 && dot < 1.0e-5) return false;
|
|
//if (dot >= 0) return false;
|
|
if (dot > -1.0e-5) return false;
|
|
|
|
|
|
float invmass1;
|
|
invmass1 = source->invmass;
|
|
|
|
float invmass2;
|
|
if (target == NULL) invmass2 = 0;
|
|
else invmass2 = target->invmass;
|
|
|
|
float t1;
|
|
if (source->invmomentofinertia == 0){
|
|
t1 = 0;
|
|
} else{
|
|
float v1[3];
|
|
vectorCross(v1, sourcecontactpoint, normal);
|
|
vectorScale(v1, source->invmomentofinertia);
|
|
float w1[3];
|
|
vectorCross(w1, v1, sourcecontactpoint);
|
|
t1 = vectorDot(normal, w1);
|
|
}
|
|
|
|
float t2;
|
|
if (target == NULL || target->invmomentofinertia == 0){
|
|
t2 = 0;
|
|
} else{
|
|
float v1[3];
|
|
vectorCross(v1, targetcontactpoint, normal);
|
|
vectorScale(v1, target->invmomentofinertia);
|
|
float w1[3];
|
|
vectorCross(w1, v1, targetcontactpoint);
|
|
t2 = vectorDot(normal, w1);
|
|
}
|
|
|
|
float denominator = invmass1 + invmass2 + t1 + t2;
|
|
|
|
float e = 1.0 - COLLISIONFRICTION;
|
|
|
|
float impulsesize = (1 + e) * dot / denominator;
|
|
|
|
//printf("%f\n", impulsesize);
|
|
|
|
float impulse[3];
|
|
vectorScale(impulse, normal, impulsesize);
|
|
|
|
|
|
float friction[3];
|
|
vectorScale(friction, normal, vectorDot(deltavelocity, normal));
|
|
vectorAdd(friction, deltavelocity);
|
|
vectorNormalize(friction);
|
|
float frictionsize = 10*KINETICFRICTION*dot/denominator;
|
|
float maxfrictionsize = 0.1*vectorLength(deltavelocity);
|
|
if (frictionsize < -maxfrictionsize) frictionsize = -maxfrictionsize;
|
|
vectorScale(friction, -frictionsize);
|
|
vectorAdd(impulse, friction);
|
|
|
|
|
|
if (target != NULL){
|
|
target->addImpulse(impulse, targetcontactpoint);
|
|
target->calculateStateVariables();
|
|
}
|
|
|
|
float speed;
|
|
float speed2[3];
|
|
|
|
if (target != NULL && source != NULL){
|
|
//float kvel[3];
|
|
//source->getVelocity(kvel);
|
|
float k = vectorLength(sourcevelocity)*0.1;
|
|
//if (k > 1) k = 1;
|
|
speed = -impulsesize*target->invmass*k;
|
|
vectorScale(speed2, impulse, target->invmass*k);
|
|
/*float kvel[3];
|
|
source->getVelocity(kvel);
|
|
float k = 0;//vectorDot(speed2, kvel);
|
|
if (k < EPSILON) k = 0;
|
|
speed *= k;
|
|
vectorScale(speed2, k);
|
|
if (k > 0) */target->hitForce(speed, speed2, source);
|
|
}
|
|
|
|
|
|
vectorScale(impulse, -1);
|
|
source->addImpulse(impulse, sourcecontactpoint);
|
|
source->calculateStateVariables();
|
|
|
|
//vectorScale(speed, source->invmass);
|
|
if (target != NULL && source != NULL){
|
|
//float kvel[3];
|
|
//target->getVelocity(kvel);
|
|
float k = vectorLength(targetvelocity)*0.1;
|
|
//if (k > 1) k = 1;
|
|
speed = -impulsesize*source->invmass*k;
|
|
vectorScale(speed2, impulse, source->invmass*k);
|
|
/*float kvel[3];
|
|
target->getVelocity(kvel);
|
|
float k = 0;//vectorDot(speed2, kvel);
|
|
if (k < EPSILON) k = 0;
|
|
speed *= k;
|
|
vectorScale(speed2, k);
|
|
if (k > 0) */source->hitForce(speed, speed2, target);
|
|
}
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
bool handleLink(ObjectLink *link){
|
|
if (!link->enabled) return false;
|
|
|
|
Object *source = link->object1;
|
|
Object *target = link->object2;
|
|
|
|
float normal[3];
|
|
float contactpoint1[3], contactpoint2[3];
|
|
source->transformPoint(contactpoint1, link->point1);
|
|
target->transformPoint(contactpoint2, link->point2);
|
|
|
|
float diff[3];
|
|
vectorSub(diff, contactpoint1, contactpoint2);
|
|
vectorNormalize(normal, diff);
|
|
|
|
float strength = vectorDot(diff, diff);
|
|
|
|
if (strength < 1.0e-5) return false;
|
|
|
|
float sourcevelocity[3], targetvelocity[3];
|
|
float sourcecontactpoint[3], targetcontactpoint[3];
|
|
|
|
vectorSub(sourcecontactpoint, contactpoint1, source->position);
|
|
source->getVelocity(sourcevelocity, sourcecontactpoint);
|
|
|
|
vectorSub(targetcontactpoint, contactpoint2, target->position);
|
|
target->getVelocity(targetvelocity, targetcontactpoint);
|
|
|
|
float deltavelocity[3];
|
|
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
|
|
float dot = vectorDot(deltavelocity, normal);
|
|
|
|
//if (fabs(dot) < EPSILON) return false;
|
|
//if (dot > -1.0e-5 && dot < 1.0e-5) return false;
|
|
//if (dot >= 0) return false;
|
|
//if (dot > -1.0e-5) return false;
|
|
|
|
|
|
float invmass1 = source->invmass;
|
|
float invmass2 = target->invmass;
|
|
|
|
float t1;
|
|
if (source->invmomentofinertia == 0){
|
|
t1 = 0;
|
|
} else{
|
|
float v1[3];
|
|
vectorCross(v1, sourcecontactpoint, normal);
|
|
vectorScale(v1, source->invmomentofinertia);
|
|
float w1[3];
|
|
vectorCross(w1, v1, sourcecontactpoint);
|
|
t1 = vectorDot(normal, w1);
|
|
}
|
|
|
|
float t2;
|
|
if (target->invmomentofinertia == 0){
|
|
t2 = 0;
|
|
} else{
|
|
float v1[3];
|
|
vectorCross(v1, targetcontactpoint, normal);
|
|
vectorScale(v1, target->invmomentofinertia);
|
|
float w1[3];
|
|
vectorCross(w1, v1, targetcontactpoint);
|
|
t2 = vectorDot(normal, w1);
|
|
}
|
|
|
|
float denominator = invmass1 + invmass2 + t1 + t2;
|
|
|
|
float impulsesize = (dot + strength*100) / denominator;
|
|
|
|
//printf("%f\n", impulsesize);
|
|
|
|
float impulse[3];
|
|
vectorScale(impulse, normal, impulsesize);
|
|
|
|
|
|
target->addImpulse(impulse, targetcontactpoint);
|
|
target->calculateStateVariables();
|
|
|
|
vectorScale(impulse, -1);
|
|
source->addImpulse(impulse, sourcecontactpoint);
|
|
source->calculateStateVariables();
|
|
|
|
|
|
return true;
|
|
}
|
|
|
|
bool checkCollisions(Object *object, float *contactnormal){
|
|
//bool collision = false;
|
|
int group = object->getCollisionGroup();
|
|
int groups = collisionlinks[group];
|
|
group = 0;
|
|
int collisions = 0;
|
|
/*float oldmomentum[3];
|
|
vectorCopy(oldmomentum, object->momentum);
|
|
|
|
while (groups){
|
|
if (groups & 1){
|
|
objectlist *node = collisionlists[group];
|
|
while (node != NULL){
|
|
Object *object2 = node->object;
|
|
node = node->next;
|
|
if (object != object2){
|
|
if (object2->geometry->checkCollision(object, contactnormal)){
|
|
collisions++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
group++;
|
|
groups >>= 1;
|
|
}
|
|
/*float temp[3];
|
|
vectorSub(temp, object->momentum, oldmomentum);
|
|
vectorScale(temp, 1.0/DT);
|
|
object->addForce(temp);*/
|
|
//vectorSub(object->
|
|
//vectorCopy(object->momentum, oldmomentum);
|
|
//printf("%i\n", collisions);
|
|
//vectorScale(object->force, 1.0/collisions);
|
|
/*if (collisions > 0){
|
|
vectorScale(object->force, 2.0/vectorLength(object->momentum));
|
|
}*/
|
|
return collisions > 0;
|
|
}
|
|
|
|
/*
|
|
* mass = 0 means infinite mass
|
|
*/
|
|
/*void collide(float *velocity1, float *velocity2,
|
|
float mass1, float mass2,
|
|
float *normal, float *newimpulse){
|
|
float deltavelocity[3];
|
|
vectorSub(deltavelocity, velocity1, velocity2);
|
|
float dot = vectorDot(deltavelocity, normal);
|
|
float massinverse1 = 0;
|
|
float massinverse2 = 0;
|
|
if (mass1 != 0) massinverse1 = 1.0 / mass1;
|
|
if (mass2 != 0) massinverse2 = 1.0 / mass2;
|
|
float impulsesize = -1 * dot / (massinverse1 + massinverse2);
|
|
vectorScale(newimpulse, normal, impulsesize);
|
|
if (dot > 0){
|
|
vectorSet(newimpulse, 0, 0, 0);
|
|
}
|
|
}*/
|
|
|
|
void collide(Object *source, Object *target, float *normal, float *contactpoint){
|
|
/* float momentum[3];
|
|
source->getMomentum(momentum);
|
|
|
|
float sourcevelocity[3], targetvelocity[3];
|
|
float sourcecontactpoint[3], targetcontactpoint[3];
|
|
|
|
//source->unTransformPoint(sourcecontactpoint, contactpoint);
|
|
//target->unTransformPoint(targetcontactpoint, contactpoint);
|
|
vectorSub(sourcecontactpoint, contactpoint, source->position);
|
|
vectorSub(targetcontactpoint, contactpoint, target->position);
|
|
|
|
source->getVelocity(sourcevelocity);//, sourcecontactpoint);
|
|
float sourcemass = source->getMass();
|
|
|
|
target->getVelocity(targetvelocity);//, targetcontactpoint);
|
|
float targetmass = target->getMass();
|
|
|
|
float deltavelocity[3];
|
|
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
|
|
float dot = vectorDot(deltavelocity, normal);
|
|
float massinverse1 = 0;
|
|
float massinverse2 = 0;
|
|
if (sourcemass != 0) massinverse1 = 1.0 / sourcemass;
|
|
if (targetmass != 0) massinverse2 = 1.0 / targetmass;
|
|
|
|
float t1;
|
|
if (source->invmomentofinertia == 0){
|
|
t1 = 0;
|
|
} else{
|
|
float v1[3];
|
|
vectorCross(v1, sourcecontactpoint, normal);
|
|
vectorScale(v1, 1.0/source->momentofinertia);
|
|
float w1[3];
|
|
vectorCross(w1, v1, sourcecontactpoint);
|
|
t1 = vectorDot(normal, w1);
|
|
}
|
|
//printf("S: %f, %f, %f\n", sourcecontactpoint[0], source->momentofinertia, v1);
|
|
|
|
float t2;
|
|
if (target->momentofinertia == 0){
|
|
t2 = 0;
|
|
} else{
|
|
float v2[3];
|
|
vectorCross(v2, targetcontactpoint, normal);
|
|
vectorScale(v2, 1.0/target->momentofinertia);
|
|
float w2[3];
|
|
vectorCross(w2, v2, targetcontactpoint);
|
|
t2 = vectorDot(normal, w2);
|
|
}
|
|
//printf("T: %f, %f, %f\n", targetcontactpoint[0], target->momentofinertia, v2);
|
|
|
|
|
|
float divisor;
|
|
divisor = massinverse1 + massinverse2;// + t1 + t2;
|
|
|
|
printf("%f, %f, %f, %f : %f\n", massinverse1, massinverse2, t1, t2, divisor);
|
|
|
|
float impulsesize = -dot / divisor;
|
|
|
|
float impulse[3];
|
|
vectorScale(impulse, normal, impulsesize);
|
|
//collide(sourcevelocity, targetvelocity, sourcemass, targetmass, normal, impulse);
|
|
|
|
//vectorAdd(source->momentum, impulse);
|
|
|
|
float *force = impulse;
|
|
|
|
//vectorScale(force, 2.0/DT);
|
|
printf("F: %f, %f, %f\n", impulse[0], impulse[1], impulse[2]);
|
|
|
|
//float cp[3];
|
|
//vectorSub(cp, contactpoint, source->position);
|
|
//source->addForce(force, sourcecontactpoint);*/
|
|
}
|
|
|
|
|
|
|
|
bool checkSphereMeshCollision(float *sphereposition, float r, Mesh *mesh, float *normal, float *contactpoint){
|
|
float linenormal[3];
|
|
float pointnormal[3];
|
|
float maxdist = 0;
|
|
bool planecollision = false;
|
|
bool linecollision = false;
|
|
bool pointcollision = false;
|
|
|
|
int i, j;
|
|
|
|
for (i = 0; i < mesh->polygoncount; i++){
|
|
class Polygon *polygon = &mesh->polygons[i];
|
|
|
|
float dist = distanceFromPlane(sphereposition, polygon->planenormal, polygon->planedistance);
|
|
if (dist < r && dist > -r){
|
|
bool directcollision = true;
|
|
for (j = 0; j < polygon->vertexcount; j++){
|
|
float *p1 = polygon->vertices[j]->position;
|
|
float *p2 = polygon->vertices[(j+1)%polygon->vertexcount]->position;
|
|
float *p3 = polygon->vertices[(j+2)%polygon->vertexcount]->position;
|
|
float v1[3], v2[3];
|
|
vectorSub(v1, p2, p1);
|
|
|
|
//Collision for polygon surface
|
|
vectorSub(v2, p3, p2);
|
|
float t1[3];
|
|
vectorProject(t1, v2, v1);
|
|
float norm[3];
|
|
vectorSub(norm, v2, t1);
|
|
vectorNormalize(norm);
|
|
|
|
//Collision for polygon edges
|
|
float newpoint[3];
|
|
vectorSub(newpoint, sphereposition, p1);
|
|
float dist2 = vectorDot(newpoint, norm);
|
|
if (dist2 < 0){
|
|
directcollision = false;
|
|
float projloc = vectorDot(newpoint, v1) / vectorDot(v1, v1);
|
|
if (projloc >= 0 && projloc <= 1){
|
|
float proj[3];
|
|
vectorScale(proj, v1, projloc);
|
|
float projorth[3];
|
|
vectorSub(projorth, newpoint, proj);
|
|
float l2 = vectorDot(projorth, projorth);
|
|
if (l2 < r*r){
|
|
vectorNormalize(linenormal, projorth);
|
|
if (dist < 0) vectorScale(linenormal, -1);
|
|
linecollision = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
//Collision for polygon vertices
|
|
float pointdiff[3];
|
|
vectorSub(pointdiff, sphereposition, p1);
|
|
float l3 = vectorDot(pointdiff, pointdiff);
|
|
if (l3 < r*r){
|
|
vectorScale(pointnormal, pointdiff, 1.0 / sqrt(l3));
|
|
if (dist < 0) vectorScale(pointnormal, -1);
|
|
pointcollision = true;
|
|
}
|
|
}
|
|
if (directcollision){
|
|
if (dist > maxdist || !planecollision){
|
|
vectorCopy(normal, polygon->planenormal);
|
|
maxdist = dist;
|
|
planecollision = true;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (planecollision){
|
|
vectorScale(contactpoint, normal, -r);
|
|
vectorAdd(contactpoint, sphereposition);
|
|
} else if (linecollision){
|
|
vectorScale(contactpoint, linenormal, -r);
|
|
vectorAdd(contactpoint, sphereposition);
|
|
vectorCopy(normal, linenormal);
|
|
} else if (pointcollision){
|
|
vectorScale(contactpoint, pointnormal, -r);
|
|
vectorAdd(contactpoint, sphereposition);
|
|
vectorCopy(normal, pointnormal);
|
|
} else{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
bool checkPointMeshCollision(float *position, Mesh *mesh, float *normal, float *contactpoint){
|
|
float maxdist = 0;
|
|
bool planecollision = false;
|
|
|
|
int i;
|
|
|
|
for (i = 0; i < mesh->polygoncount; i++){
|
|
class Polygon *polygon = &mesh->polygons[i];
|
|
|
|
float dist = distanceFromPlane(position, polygon->planenormal, polygon->planedistance);
|
|
if (dist < 0){
|
|
bool directcollision = true;
|
|
/*for (j = 0; j < polygon->vertexcount; j++){
|
|
float *p1 = polygon->vertices[j]->position;
|
|
float *p2 = polygon->vertices[(j+1)%polygon->vertexcount]->position;
|
|
float *p3 = polygon->vertices[(j+2)%polygon->vertexcount]->position;
|
|
float v1[3], v2[3];
|
|
vectorSub(v1, p2, p1);
|
|
|
|
//Collision for polygon surface
|
|
vectorSub(v2, p3, p2);
|
|
float t1[3];
|
|
vectorProject(t1, v2, v1);
|
|
float norm[3];
|
|
vectorSub(norm, v2, t1);
|
|
vectorNormalize(norm);
|
|
|
|
//Collision for polygon edges
|
|
float newpoint[3];
|
|
vectorSub(newpoint, position, p1);
|
|
float dist2 = vectorDot(newpoint, norm);
|
|
if (dist2 < 0) directcollision = false;
|
|
}*/
|
|
if (directcollision){
|
|
if (dist > maxdist || !planecollision){
|
|
vectorCopy(normal, polygon->planenormal);
|
|
maxdist = dist;
|
|
planecollision = true;
|
|
}
|
|
}
|
|
} else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (planecollision){
|
|
vectorCopy(contactpoint, position);
|
|
} else{
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
#define MAXPOLYGONS 1000
|
|
|
|
struct tracehit{
|
|
float t;
|
|
Polygon *polygon;
|
|
};
|
|
|
|
bool tracePlane(tracehit *result, float *origin, float *ray, class Polygon *polygon){
|
|
float *normal = polygon->planenormal;
|
|
float D = polygon->planedistance;
|
|
float denominator = vectorDot(normal, ray);
|
|
if (denominator == 0){
|
|
if (vectorDot(normal, origin) > 0) result->t = 1000000;
|
|
else result->t = -1000000;
|
|
result->polygon = polygon;
|
|
return true;
|
|
}
|
|
|
|
float t = - (vectorDot(normal, origin) + D) / denominator;
|
|
|
|
result->t = t;
|
|
result->polygon = polygon;
|
|
|
|
//if (t < 0 || t > 1) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
Edge *findSharingEdge(class Polygon *p1, class Polygon *p2){
|
|
//printf("Edges:\n");
|
|
int i, j;
|
|
for (i = 0; i < p1->vertexcount; i++){
|
|
//printf("%p\n", p1->edges[i]);
|
|
for (j = 0; j < p2->vertexcount; j++){
|
|
if (p1->edges[i] == p2->edges[j]) return p1->edges[i];
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/*Polygon *findSharingPolygon(Mesh *mesh, class Polygon *p1, class Polygon *p2){
|
|
//printf("Edges:\n");
|
|
int i, j;
|
|
for (i = 0; i < p1->vertexcount; i++){
|
|
//printf("%p\n", p1->edges[i]);
|
|
for (j = 0; j < p2->vertexcount; j++){
|
|
if (p1->edges[i] == p2->edges[j]) return p1->edges[i];
|
|
}
|
|
}
|
|
return NULL;
|
|
}*/
|
|
Polygon *findNearestPolygon(class Polygon *polygon, float *point){
|
|
int i;
|
|
float mindist = 0;
|
|
Polygon *nearestpolygon = NULL;
|
|
for (i = 0; i < polygon->edgecount; i++){
|
|
Edge *edge = polygon->edges[i];
|
|
Polygon *polygon2 = edge->p1;
|
|
if (polygon2 == polygon) polygon2 = edge->p2;
|
|
float newdist = distanceFromPlane(point, polygon2->planenormal, polygon2->planedistance);
|
|
if (newdist > 0) return NULL;
|
|
if (mindist == 0 || newdist > mindist){
|
|
mindist = newdist;
|
|
nearestpolygon = polygon2;
|
|
}
|
|
}
|
|
return nearestpolygon;
|
|
}
|
|
|
|
bool checkEdgeMeshCollision(float *p1, float *p2, Mesh *mesh, float *normal, float *contactpoint){
|
|
float ray[3];
|
|
vectorSub(ray, p2, p1);
|
|
|
|
float maxdist = 0;
|
|
bool collision = false;
|
|
|
|
int i, j;
|
|
|
|
tracehit hits[MAXPOLYGONS];
|
|
int hitcount = 0;
|
|
|
|
for (i = 0; i < mesh->polygoncount; i++){
|
|
class Polygon *polygon = &mesh->polygons[i];
|
|
|
|
if (tracePlane(&hits[hitcount], p1, ray, polygon)){
|
|
hitcount++;
|
|
}
|
|
}
|
|
|
|
if (hitcount < 2) return false;
|
|
|
|
for (i = 1; i < hitcount; i++){
|
|
for (j = i; j > 0; j--){
|
|
if (hits[j].t < hits[j-1].t){
|
|
float tempt = hits[j].t;
|
|
hits[j].t = hits[j-1].t;
|
|
hits[j-1].t = tempt;
|
|
class Polygon *tempp = hits[j].polygon;
|
|
hits[j].polygon = hits[j-1].polygon;
|
|
hits[j-1].polygon = tempp;
|
|
} else break;
|
|
}
|
|
}
|
|
|
|
int negative = -1, positive = -1;
|
|
|
|
|
|
for (i = 0; i < hitcount; i++){
|
|
float t = hits[i].t;
|
|
class Polygon *polygon = hits[i].polygon;
|
|
|
|
float dot = vectorDot(ray, polygon->planenormal);
|
|
|
|
if (dot > 0 && positive == -1) positive = i;
|
|
if (dot < 0) negative = i;
|
|
|
|
if (dot < 0 && positive != -1) return false;
|
|
}
|
|
|
|
if (negative == -1 || positive == -1) return false;
|
|
|
|
/*for (i = 0; i < hitcount; i++){
|
|
float t = hits[i].t;
|
|
class Polygon *polygon = hits[i].polygon;
|
|
|
|
float dot = vectorDot(ray, polygon->planenormal);
|
|
|
|
printf("%f ", dot);
|
|
}
|
|
printf("\n");*/
|
|
|
|
if (hits[negative].t < 0 || hits[positive].t > 1) return false;
|
|
|
|
Edge *edge2 = findSharingEdge(hits[negative].polygon, hits[positive].polygon);
|
|
|
|
//fflush(stdout);
|
|
float cp1[3], cp2[3];
|
|
vectorScale(cp1, ray, hits[negative].t);
|
|
vectorAdd(cp1, p1);
|
|
vectorScale(cp2, ray, hits[positive].t);
|
|
vectorAdd(cp2, p1);
|
|
|
|
if (edge2 != NULL){
|
|
|
|
/*float ev1[3];
|
|
vectorSub(ev1, edge2->v2->position, edge2->v1->position);
|
|
vectorCross(normal, ev1, ray);
|
|
vectorScale(normal, vectorDot(normal, hits[positive].polygon->planenormal));
|
|
vectorNormalize(normal);
|
|
|
|
float at = (hits[negative].t + hits[positive].t) / 2;
|
|
vectorScale(contactpoint, ray, at);
|
|
vectorAdd(contactpoint, p1);*/
|
|
|
|
float dot1 = fabs(vectorDot(ray, hits[negative].polygon->planenormal));
|
|
float dot2 = fabs(vectorDot(ray, hits[positive].polygon->planenormal));
|
|
|
|
if (dot1 > dot2){
|
|
//vectorScale(contactpoint, ray, hits[negative].t);
|
|
//vectorAdd(contactpoint, p1);
|
|
vectorCopy(contactpoint, cp1);
|
|
vectorCopy(normal, hits[positive].polygon->planenormal);
|
|
} else{
|
|
//vectorScale(contactpoint, ray, hits[positive].t);
|
|
//vectorAdd(contactpoint, p1);
|
|
vectorCopy(contactpoint, cp2);
|
|
vectorCopy(normal, hits[negative].polygon->planenormal);
|
|
}
|
|
} else{
|
|
Polygon *polygon = findNearestPolygon(hits[negative].polygon, cp1);
|
|
if (polygon != NULL){
|
|
/*vectorCopy(contactpoint, cp1);
|
|
vectorAdd(contactpoint, cp2);
|
|
vectorScale(contactpoint, 0.5);*/
|
|
float at = (hits[negative].t + hits[positive].t) / 2;
|
|
vectorScale(contactpoint, ray, at);
|
|
vectorAdd(contactpoint, p1);
|
|
|
|
vectorCopy(normal, polygon->planenormal);
|
|
} else{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
|
|
//shotsound->play();
|
|
return true;
|
|
} |