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; }