void Plane::getSegmentPlaneIntersection(Vector *p1, Vector *p2,
VectorPtrArray *intersections, double p1Dot, double p2Dot) {
double d1 = (p1Dot == 0.0f ? distanceFromPlane(p1) : p1Dot);
double d2 = (p2Dot == 0.0f ? distanceFromPlane(p2) : p2Dot);
bool onPlaneP1 = abs(d1) < this->eps;
bool onPlaneP2 = abs(d2) < this->eps;
if (onPlaneP1) {
addNewIfNotAlrdy(intersections,p1);
}
if (onPlaneP2) {
addNewIfNotAlrdy(intersections,p2);
}
if ((onPlaneP1 && onPlaneP2) || (d1 * d1 > eps)) {
return;
}
double d = d1 / (d1 - d2);
// Vector dp21 = *p2 - *p1;
// intersections->push_back(new Vector(*p1 + (dp21 * d)) );
Vector *dp21 = new Vector(p2);
dp21->minus(*p1);
dp21->scale(d)->add(*p1);
intersections->push_back( dp21 );
}
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;
}
/*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 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;
}
