/* 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); }