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