/*
Returns true if vector (alx, aly)->(aux, auy)
collides with rectangular obstacle
*/
bool obstacleCollision(double alx, double aly, double aux, double auy, Obstacles* o){
double lx = o->lx;
double ly = o->ly;
double ux = o->ux;
double uy = o->uy;
if (LineCollision(alx, aly, aux, auy, lx, ly, lx, uy) ||
LineCollision(alx, aly, aux, auy, lx, ly, ux, ly) ||
LineCollision(alx, aly, aux, auy, ux, ly, ux, uy) ||
LineCollision(alx, aly, aux, auy, lx, uy, ux, uy)) {
//cout << "found collision" << endl;
return true;
}
return false;
}
SquareSides SquareCollisions::intersectLineWithSquare(glm::vec2 & squarePos, float halfSideLength, glm::vec2 & P1, glm::vec2 & P2)
{
if (LineCollision(glm::vec2(squarePos.x + halfSideLength, squarePos.y + halfSideLength), glm::vec2(squarePos.x - halfSideLength, squarePos.y + halfSideLength), P1, P2))
{
return top;
}
if (LineCollision(glm::vec2(squarePos.x + halfSideLength, squarePos.y - halfSideLength), glm::vec2(squarePos.x - halfSideLength, squarePos.y - halfSideLength), P1, P2))
{
return bottom;
}
if (LineCollision(glm::vec2(squarePos.x - halfSideLength, squarePos.y + halfSideLength), glm::vec2(squarePos.x - halfSideLength, squarePos.y - halfSideLength), P1, P2))
{
return right;
}
if (LineCollision(glm::vec2(squarePos.x + halfSideLength, squarePos.y + halfSideLength), glm::vec2(squarePos.x + halfSideLength, squarePos.y - halfSideLength), P1, P2))
{
return left;
}
return none;
}
glm::vec3 ColliderManager::getpointOfCollision(Collider3D &A, Collider3D &B, glm::vec3 mtv)
{
glm::vec3 n = glm::normalize(mtv);
float tolerance = 0.01f;
int totalSuspects1 = 1, totalSuspects2 = 1;
// the transform matrices for each object
glm::mat4 transform1, transform2;
glm::vec3 pointofCollision;
glm::vec3 closestPointsA[2], ClosestPointsB[2];
//transform matrices consist of translation and rotation matrix of each respective object
transform1 = A.transform->returnModelMatrix() ;
transform2 = B.transform->returnModelMatrix() ;
float pointsA, pointsB;
std::vector<glm::vec3>::iterator it;
//Set the min to maximum possible value, and max to minimum possible value
float max = -99.0f, min = 99.0f;
int i;
for (it = A.vertexSet.begin(); it != A.vertexSet.end(); it++)
{
pointsA = (glm::dot(glm::vec3(transform1*(glm::vec4(*it, 1.0f))), n));
if (abs(max - pointsA) <= tolerance)
{
closestPointsA[1] = *it; // If any other point lies close to the given point within the tolerance level, increment the total number of suspects.
totalSuspects1++; // update the closest poitns set
}
else
if (max < pointsA) // If another point is found which is closer then reset the count and store the 2 closest points
{
max = pointsA;
totalSuspects1 = 1;
pointofCollision = *it;
closestPointsA[0] = pointofCollision;
closestPointsA[1] = pointofCollision;
}
}
//Point to face or edge collision, with point of object A piercing object B
if (totalSuspects1 == 1)
{
return glm::vec3(transform1*(glm::vec4(pointofCollision, 1.0f)));
}
for (it = B.vertexSet.begin(); it != B.vertexSet.end(); it++)
{
pointsB = (glm::dot(glm::vec3(transform2*(glm::vec4(*it, 1.0f))), n));
if (abs(min - pointsB) <= tolerance)
{
ClosestPointsB[1] = *it; // If any other point lies close to the given point within the tolerance level, increment the total number of suspects.
totalSuspects2++; // update the closest poitns set
}
else
if (min > pointsB)
{
min = pointsB;
totalSuspects2 = 1; // If another point is found which is closer then reset the count and store the 2 closest points
pointofCollision = *it;
ClosestPointsB[0] = pointofCollision;
ClosestPointsB[1] = pointofCollision;
}
}
//Point to face or edge collision, with point of object B piercing object A
if (totalSuspects2 == 1)
{
return glm::vec3(transform2*(glm::vec4(pointofCollision, 1.0f)));
}
//If the number of suspects is exactly 2 on both sides, then the collision is edge to edge
//Edge to edge collision
if (totalSuspects1 == 2 && totalSuspects2 == 2)
{
Line l1, l2; // Create two lines with the values we stored earlier in the previous steps.
l1.point1 = glm::vec3(transform1 * glm::vec4(closestPointsA[0], 1.0f));
l1.point2 = glm::vec3(transform1 * glm::vec4(closestPointsA[1], 1.0f));
l2.point1 = glm::vec3(transform2 * glm::vec4(ClosestPointsB[0], 1.0f));
l2.point2 = glm::vec3(transform2 * glm::vec4(ClosestPointsB[1], 1.0f));
return LineCollision(l1, l2); // Pass them to deduce the point of collision
}
//If we have reached this point, then the face of one of the objects is colliding with the edge or the face of
//the other object, as the suspects count has to be greater than 2 for both the objects.
// edge-face collision or face face collision;
//here v1,v2 and v3 are the basis vector we will derive from n, using grahm-schmidt's process
//v1 = x-axis
//v2 = y-axis
//v3 = z-axis
glm::vec3 v1, v2, v3;
v1 = glm::vec3(1.0f, 0.0f, 0.0f) - n * n.x; // Subtract the component of the vector n on X-axis to get the first basis vector.
v2 = glm::vec3(0.0f, 1.0f, 0.0f) - n*n.y - v1 * v1.y; // Subtract the component of the vector n on Y axis and v1 on Yaxis to get the second vector
v3 = glm::vec3(0.0f, 0.0f, 1.0f) - n*n.z - (v1* v1.z) - (v2* v2.z); // subtract the component of the vector n on Z axis and v1 on Z axis and v2 on Zaxis to get the third basis vector
//We only need 2 basis vectors, So we choose 2 of those which are not 0.
// Since we are dealing with a small system here, FLT_EPSILON is not enough of a error margin, so we multiply it by 2 to increase it.
//This section of code basically, swaps the three vectors v1,v2,and v3 such that the non-zero vectors are always stored in v1 and v2
if (glm::length(v1) < 2.0f*FLT_EPSILON)
{
if (glm::length(v3) > 2.0f*FLT_EPSILON)
v1 = v3;
}
else if (glm::length(v2) < 2.0f * FLT_EPSILON)
{
if (glm::length(v3) > 2.0f * FLT_EPSILON)
v2 = v3;
}
v1 = glm::normalize(v1);
v2 = glm::normalize(v2);
glm::vec3 component1, component2, component3, POC;
//get the component along the two vectors and add it.
component1 = getPOCin1D(A, B, v1);
component2 = getPOCin1D(A, B, v2);
POC = component1 + component2;
//adding the resultant with the distance of the plane from the origin multiplied by the normal(plane) will give us the position of the point.
float d = glm::dot(n, (glm::vec3(transform2*(glm::vec4(ClosestPointsB[0], 1.0f)))));
return POC + d * n;
}
void Corpse::ApplyConstraint(float dt) {
SPADES_MARK_FUNCTION();
AngularMomentum(0,
Torso1, Torso2);
AngularMomentum(1,
Torso2, Torso3);
AngularMomentum(2,
Torso3, Torso4);
AngularMomentum(3,
Torso4, Torso1);
AngularMomentum(4,
Torso1, Arm1);
AngularMomentum(5,
Torso2, Arm2);
AngularMomentum(6,
Torso3, Leg1);
AngularMomentum(7,
Torso4, Leg2);
Spring(Torso1, Torso2,
0.8f, dt);
Spring(Torso3, Torso4,
0.8f, dt);
Spring(Torso1, Torso4,
0.9f, dt);
Spring(Torso2, Torso3,
0.9f, dt);
Spring(Torso1, Torso3,
1.204f, dt);
Spring(Torso2, Torso4,
1.204f, dt);
Spring(Arm1, Torso1,
1.f, dt);
Spring(Arm2, Torso2,
1.f, dt);
Spring(Leg1, Torso3,
1.f, dt);
Spring(Leg2, Torso4,
1.f, dt);
AngleSpring(Torso1,
Arm1, Torso3,
-1.f, 0.6f, dt);
AngleSpring(Torso2,
Arm2, Torso4,
-1.f, 0.6f, dt);
AngleSpring(Torso3,
Leg1, Torso2,
-1.f, -0.2f, dt);
AngleSpring(Torso4,
Leg2, Torso1,
-1.f, -0.2f, dt);
Spring(Torso1, Torso2, Head,
.6f, dt);
/*
AngleSpring(Torso1,
Torso2, Head,
0.5f, 1.f, dt);
AngleSpring(Torso2,
Torso1, Head,
0.5f, 1.f, dt);
*/
LineCollision(Torso1, Torso2, dt);
LineCollision(Torso2, Torso3, dt);
LineCollision(Torso3, Torso4, dt);
LineCollision(Torso4, Torso1, dt);
LineCollision(Torso1, Torso3, dt);
LineCollision(Torso2, Torso4, dt);
LineCollision(Torso1, Arm1, dt);
LineCollision(Torso2, Arm2, dt);
LineCollision(Torso3, Leg1, dt);
LineCollision(Torso4, Leg2, dt);
return;
AngleSpring(Torso4,
Torso1, Head,
0.5f, 1.f, dt);
AngleSpring(Torso3,
Torso2, Head,
0.5f, 1.f, dt);
AngleSpring(Torso4,
Torso2, Head,
0.5f, 1.f, dt);
AngleSpring(Torso3,
Torso1, Head,
0.5f, 1.f, dt);
}
