std::vector<Triangle> DelaunayAlgorithm::checkPointInTriangles(const cocos2d::Point&p, const std::vector<Triangle>& triangles) { std::vector<Triangle> results; for (auto triangle : triangles) { if (checkPointInTriangle(p, triangle)) { results.push_back(triangle); } } return results; }
// Assumes: p1,p2 and p3 are given in ellisoid space: void checkTriangle(CollisionPacket* colPackage,VECTOR p1,VECTOR p2,VECTOR p3) { #ifndef __GNUC__ // Make the plane containing this triangle. PLANE trianglePlane(p1,p2,p3); // Is triangle front-facing to the velocity vector? // We only check front-facing triangles if (trianglePlane.isFrontFacingTo(colPackage->normalizedVelocity) || (! isBackCulling)) { // Get interval of plane intersection: double t0, t1; bool embeddedInPlane = false; // Calculate the signed distance from sphere // position to triangle plane double signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage->basePoint); // cache this as we're going to use it a few times below: float normalDotVelocity = trianglePlane.normal.dot(colPackage->velocity); // if sphere is travelling parrallel to the plane: if (normalDotVelocity == 0.0f) { if (fabs(signedDistToTrianglePlane) >= 1.0f) { // Sphere is not embedded in plane. // No collision possible: return; } else { // sphere is embedded in plane. // It intersects in the whole range [0..1] embeddedInPlane = true; t0 = 0.0; t1 = 1.0; } } else { // N dot D is not 0. Calculate intersection interval: t0=(-1.0-signedDistToTrianglePlane)/normalDotVelocity; t1=( 1.0-signedDistToTrianglePlane)/normalDotVelocity; // Swap so t0 < t1 if (t0 > t1) { double temp = t1; t1 = t0; t0 = temp; } // Check that at least one result is within range: if (t0 > 1.0f || t1 < 0.0f) { // Both t values are outside values [0,1] // No collision possible: return; } // Clamp to [0,1] if (t0 < 0.0) t0 = 0.0; if (t1 < 0.0) t1 = 0.0; if (t0 > 1.0) t0 = 1.0; if (t1 > 1.0) t1 = 1.0; } // OK, at this point we have two time values t0 and t1 // between which the swept sphere intersects with the // triangle plane. If any collision is to occur it must // happen within this interval. VECTOR collisionPoint; bool foundCollison = false; float t = 1.0; // First we check for the easy case - collision inside // the triangle. If this happens it must be at time t0 // as this is when the sphere rests on the front side // of the triangle plane. Note, this can only happen if // the sphere is not embedded in the triangle plane. if (!embeddedInPlane) { VECTOR planeIntersectionPoint = (colPackage->basePoint-trianglePlane.normal) + colPackage->velocity*t0; if (checkPointInTriangle(planeIntersectionPoint,p1,p2,p3)) { foundCollison = true; t = t0; collisionPoint = planeIntersectionPoint; } } // if we haven't found a collision already we'll have to // sweep sphere against points and edges of the triangle. // Note: A collision inside the triangle (the check above) // will always happen before a vertex or edge collision! // This is why we can skip the swept test if the above // gives a collision! if (foundCollison == false) { // some commonly used terms: VECTOR velocity = colPackage->velocity; VECTOR base = colPackage->basePoint; float velocitySquaredLength = velocity.squaredLength(); float a,b,c; // Params for equation float newT; // For each vertex or edge a quadratic equation have to // be solved. We parameterize this equation as // a*t^2 + b*t + c = 0 and below we calculate the // parameters a,b and c for each test. // Check against points: a = velocitySquaredLength; // P1 b = 2.0*(velocity.dot(base-p1)); c = (p1-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p1; } // P2 b = 2.0*(velocity.dot(base-p2)); c = (p2-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p2; } // P3 b = 2.0*(velocity.dot(base-p3)); c = (p3-base).squaredLength() - 1.0; if (getLowestRoot(a,b,c, t, &newT)) { t = newT; foundCollison = true; collisionPoint = p3; } // Check agains edges: // p1 -> p2: VECTOR edge = p2-p1; VECTOR baseToVertex = p1 - base; float edgeSquaredLength = edge.squaredLength(); float edgeDotVelocity = edge.dot(velocity); float edgeDotBaseToVertex = edge.dot(baseToVertex); // Calculate parameters for equation a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; // Does the swept sphere collide against infinite edge? if (getLowestRoot(a,b,c, t, &newT)) { // Check if intersection is within line segment: float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { // intersection took place within segment. t = newT; foundCollison = true; collisionPoint = p1 + edge*f; } } // p2 -> p3: edge = p3-p2; baseToVertex = p2 - base; edgeSquaredLength = edge.squaredLength(); edgeDotVelocity = edge.dot(velocity); edgeDotBaseToVertex = edge.dot(baseToVertex); a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; if (getLowestRoot(a,b,c, t, &newT)) { float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { t = newT; foundCollison = true; collisionPoint = p2 + edge*f; } } // p3 -> p1: edge = p1-p3; baseToVertex = p3 - base; edgeSquaredLength = edge.squaredLength(); edgeDotVelocity = edge.dot(velocity); edgeDotBaseToVertex = edge.dot(baseToVertex); a = edgeSquaredLength*-velocitySquaredLength + edgeDotVelocity*edgeDotVelocity; b = edgeSquaredLength*(2*velocity.dot(baseToVertex)) - 2.0*edgeDotVelocity*edgeDotBaseToVertex; c = edgeSquaredLength*(1-baseToVertex.squaredLength()) + edgeDotBaseToVertex*edgeDotBaseToVertex; if (getLowestRoot(a,b,c, t, &newT)) { float f=(edgeDotVelocity*newT-edgeDotBaseToVertex) / edgeSquaredLength; if (f >= 0.0 && f <= 1.0) { t = newT; foundCollison = true; collisionPoint = p3 + edge*f; } } } // Set result: if (foundCollison == true) { // distance to collision: 't' is time of collision float distToCollision = t*colPackage->velocity.length(); // Does this triangle qualify for the closest hit? if (distToCollision < colPackage->nearestDistance) { // Collision information nessesary for sliding colPackage->nearestDistance = distToCollision; colPackage->intersectionPoint=collisionPoint; colPackage->foundCollision = true; // trigger collection of triangle index when leave colPackage->triangleindex = -2; } } } // if not backface #endif }
void inline collSpherePlanes() { // p= startpunkt // q=endpunkt // a,b,c = Dreieckspunkte // u,v,w baryzentrische koordinaten zum Schnittpunkt s // s = u*a + v*b + w*c triangle tri; bowl *b; float u,v,w; int check = 0; Vector s(3); // schnittpunkt mit dreieck float pace = 0; float lambda = 0; for(int h = 0; h < bcount; h++) { b = &bowls[h]; if(b->fixed) { continue; } for(int i = 0; i < objects.size(); i++) { for(int j = 0; j < objects[i].triangles.size(); j++) { Vector dir(3); dir[0] = b->pace[0]; dir[1] = b->pace[1]; dir[2] = b->pace[2]; normalizeVector(&dir); float len = getVectorLen(dir); tri = objects[i].triangles[j]; // tri um länge des radius heranholen float sign = (scalarProd(&dir, &tri.normal) < 0) ? 1 : -1; tri.a[0] += b->radius*tri.normal[0]*sign; tri.a[1] += b->radius*tri.normal[1]*sign; tri.a[2] += b->radius*tri.normal[2]*sign; tri.b[0] += b->radius*tri.normal[0]*sign; tri.b[1] += b->radius*tri.normal[1]*sign; tri.b[2] += b->radius*tri.normal[2]*sign; tri.c[0] += b->radius*tri.normal[0]*sign; tri.c[1] += b->radius*tri.normal[1]*sign; tri.c[2] += b->radius*tri.normal[2]*sign; // broad phase: pace = getVectorLen(b->pace); // naiver test mit unendlicher ebene check = checkIntersectRayPlane(&tri, &b->oldPos, &b->pace, &lambda); if (check == 0 || lambda > pace) { continue; } std::cout << lambda << "\r\n"; //check = IntersectLineTriangle(b->oldPos, b->pos, tri.a, tri.b, tri.c, u,v,w, &tri); s[0] = b->oldPos[0] + lambda*dir[0]; s[1] = b->oldPos[1] + lambda*dir[1]; s[2] = b->oldPos[2] + lambda*dir[2]; check = checkPointInTriangle(s, &tri); Vector face = s - b->oldPos; normalizeVector(&face); float dot = scalarProd(&face, &dir); if(check == 1 && dot > 0) { // schauen, ob schnittpunkt überschritten wurde if(lambda <= pace) { Vector m(3); m[0] = b->oldPos[0] + b->pace[0]/2; m[1] = b->oldPos[1] + b->pace[1]/2; m[2] = b->oldPos[2] + b->pace[2]/2; // schauen, ob schnittpunkt im radius ist float d = getDistance(s,m); if(d < pace) { // kollision Vector out(3); float subLen = abs(getDistance(s, b->pos)); getOutVector(dir, tri.normal, &out); b->pos[0] = s[0] + subLen * out[0]; // + out[0] * subLen; b->pos[1] = s[1] + subLen * out[1]; // + out[1] * subLen; b->pos[2] = s[2] + subLen * out[2]; // + out[2] * subLen; // neue schrittweite // gleitanteil Vector slide = out - tri.normal; float outDotN = abs(scalarProd(&out,&tri.normal)); slide[0]= (out[0] - outDotN*tri.normal[0]); slide[1]= (out[1] - outDotN*tri.normal[1]); slide[2]= (out[2] - outDotN*tri.normal[2]); out[0] = tri.normal[0] + 8*slide[0]; out[1] = tri.normal[1] + 8*slide[1]; out[2] = tri.normal[2] + 8*slide[2]; normalizeVector(&out); // kugel fällt durch das mesh durch, wenn aktiviert if(friction) { pace *= b->friction; } if(pace <= 0.001 && fix) { pace = 0; b->fixed = true; } b->pace[0] = out[0] * pace; b->pace[1] = out[1] * pace; b->pace[2] = out[2] * pace; } } } } } } }
int checkCollide(glm::vec3 N,glm::vec3 p,glm::vec3 vel, glm::vec3 ta, glm::vec3 tb, glm::vec3 tc, glm::vec3 &result, float &distance){ // Figure out two times, t0 and t1. // By finding the signed distance to the plane at two places. // We want to know if a) it intersects the plane of the triangle // b) if it is within the bounds of the triangle (ta/tb/tc) // c) or if it's on the edges / points of the triangles. // This function returns 1 if it does intersect, or 0 otherwise. // get interval of intersection float t0, t1; bool embedded = false; // calc distance float distToPlane = SignedDistance(N,p,ta); float nDvel = glm::dot(N,vel); if (nDvel < 0.0f){ if (fabs(distToPlane) >= 1.0f){ return 0; } else { embedded = true; t0 = 0.0; t1 = 1.0; } } else{ t0 = (-1.0-distToPlane)/nDvel; t1 = (1.0 - distToPlane)/nDvel; if (t0 > t1){ float temp = t1; t1 = t0; t0 = temp; } if (t0>1.0f || t1< 0.0f){ return 0; } if (t0<0.0) t0 = 0.0; if (t1<0.0) t1 = 0.0; if (t0>1.0) t0 = 1.0; if (t1>1.0) t1 = 1.0; } glm::vec3 colPoint; bool foundCol = false; float t = 1.0; glm::vec3 planeIntersect = (p-N + t0*vel); if (!embedded){ if(checkPointInTriangle(planeIntersect, ta, tb, tc)){ foundCol = true; t = t0; colPoint = planeIntersect; distance = t*glm::length(vel); result = colPoint; return 1; } return 0; } /// VERY IMPORTANT. This is where it checks for intersection in the area of the triangle. else { if(checkPointInTriangle(planeIntersect, ta, tb, tc)){ foundCol = true; t = t0; colPoint = planeIntersect; distance = t*glm::length(vel); result = colPoint; return 1; } } if (foundCol ==false){ glm::vec3 base = p; float velSquared = SquaredLength(vel); float a,b,c; float newT; a = velSquared; // for ta b = 2.0f*glm::dot(vel,base-ta); c = SquaredLength(ta - base) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = ta; distance = t*glm::length(vel); result = colPoint; return 1; } // for tb b = 2.0f*glm::dot(vel,base-tb); c = SquaredLength(tb - base) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = tb; distance = t*glm::length(vel); result = colPoint; return 1; } // for tc b = 2.0f*glm::dot(vel,base-tc); c = SquaredLength((tc - base)) -1.0; if (getLowestRoot(a,b,c,t, &newT)){ t = newT; foundCol = true; colPoint = tc; distance = t*glm::length(vel); result = colPoint; return 1; } // now edges // ta -> tb glm::vec3 edge = tb-ta; glm::vec3 baseToVertex = ta - base; float edgeSquared = SquaredLength(edge); float edgeDotVel = glm::dot(edge, vel); float edgeDotBaseToVert = glm::dot(edge, baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint = ta + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } // tb -> tc edge = tc-tb; baseToVertex = tb - base; edgeSquared = SquaredLength(edge); edgeDotVel = glm::dot(edge, vel); edgeDotBaseToVert = glm::dot(edge,baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint =tb + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } // tc -> ta edge = ta-tc; baseToVertex = tc - base; edgeSquared = SquaredLength(edge); edgeDotVel = glm::dot(edge, vel); edgeDotBaseToVert = glm::dot(edge,baseToVertex); a = edgeSquared*-velSquared + edgeDotVel*edgeDotVel; b = edgeSquared*(2*glm::dot(vel,baseToVertex))-2.0*edgeDotVel*edgeDotBaseToVert; c = edgeSquared*(1-SquaredLength(baseToVertex))+edgeDotBaseToVert*edgeDotBaseToVert; if (getLowestRoot(a,b,c,t,&newT)){ float f = (edgeDotVel*newT - edgeDotBaseToVert)/edgeSquared; if(f>=0.0 && f<=1.0){ t = newT; foundCol = true; colPoint =tc + f*edge; distance = t*glm::length(vel); result = colPoint; return 1; } } } distance = t*glm::length(vel); return 0; }