// Collision reaction. // Return true if collision is considered stable (no position modifications occured) public virtual bool OnCollision(PhysicWorldModel world, PhysicPointModel pointModel, PhysicPlaneModel planeModel, FixedVector3 intersection) { // if (pointModel.position == intersection){ // // Nothing to change // return true; // } // Pick one of two methods depending on the plane's normal angle against up vector FixedFloat planeAngle = FixedVector3.Angle(planeModel.normal, FixedVector3.Up); // If it's too much inclined, use natural reaction if (planeAngle > FixedFloat.PI * 0.4) { // check direction against 4 walls FixedFloat maxDelta = FixedFloat.PI * 0.4; planeAngle = FixedVector3.Angle(planeModel.normal, FixedVector3.Left); if (planeAngle <= maxDelta || planeAngle >= FixedFloat.PI - maxDelta) { // collision in the X axis FixedFloat inpactX = pointModel.GetVelocity().X - planeModel.GetVelocity().X; if (FixedFloat.Abs(inpactX) > FixedFloat.Abs(newCollisionInpact.X)) { newCollisionInpact.X = inpactX; } } planeAngle = FixedVector3.Angle(planeModel.normal, FixedVector3.Forward); if (planeAngle <= maxDelta || planeAngle >= FixedFloat.PI - maxDelta) { // collision in the Z axis FixedFloat inpactZ = pointModel.GetVelocity().Z - planeModel.GetVelocity().Z; if (FixedFloat.Abs(inpactZ) > FixedFloat.Abs(newCollisionInpact.Z)) { newCollisionInpact.Z = inpactZ; } } return(CollisionNaturalReaction(world, pointModel, planeModel, intersection)); } else { // Otherwise we're hitting the ground, do not slide FixedFloat inpactY = pointModel.GetVelocity().Y - planeModel.GetVelocity().Y; if (FixedFloat.Abs(inpactY) > FixedFloat.Abs(newCollisionInpact.Y)) { newCollisionInpact.Y = inpactY; } // We use a lot of arguments here just to avoid recalculating them return(CollisionGroundReaction(world, pointModel, planeModel, intersection)); } }
// Compute the intersection point against a line segment public static bool CheckIntersection(PhysicPlaneModel planeModel, FixedVector3 pos1, FixedVector3 pos2, FixedVector3 stepTolerance, out FixedVector3 intersection) { // plane may be moving, sum velocity to initial point position pos1 += planeModel.GetVelocity(); // Check bounding box intersection, including step tolerance if (!BoxIntersection(planeModel, pos1 + stepTolerance, pos2)) { intersection = FixedVector3.Zero; return(false); } // check collision with the hiperplane FixedVector3 pointDeltaPos = pos2 - pos1; if (pointDeltaPos.Magnitude == 0) { // The point is not moving relatively to the plane intersection = FixedVector3.Zero; return(false); } FixedVector3 pos1ToOrigin = planeModel.origin - pos1; FixedFloat dotDeltaPosNormal = FixedVector3.Dot(pointDeltaPos, planeModel.normal); if (dotDeltaPosNormal >= 0) { // Point moving away from the plane intersection = FixedVector3.Zero; return(false); } // Find intersection location in the deltapos vector FixedFloat t = FixedVector3.Dot(pos1ToOrigin, planeModel.normal) / dotDeltaPosNormal; // a small delta due to precision errors // based on deltaPos magnitude (the smaller the magnitude the higher the error) FixedFloat error = 0.01 / pointDeltaPos.Magnitude; if (t < -error) { // falling through the plane, try step tolerance to recover pos1 += stepTolerance; pointDeltaPos = pos2 - pos1; pos1ToOrigin = planeModel.origin - pos1; dotDeltaPosNormal = FixedVector3.Dot(pointDeltaPos, planeModel.normal); t = FixedVector3.Dot(pos1ToOrigin, planeModel.normal) / dotDeltaPosNormal; error = 0.01 / pointDeltaPos.Magnitude; } // give some tolerance if (t < -error || t > 1 + error) { // not colliding intersection = FixedVector3.Zero; return(false); } intersection = pos1 + t * pointDeltaPos; // Check if intersection point is inside the plane FixedFloat anglesSum = FixedFloat.Zero; FixedVector3 originVector = planeModel.origin - intersection; FixedVector3 vec1 = originVector; FixedVector3 vec2 = FixedVector3.Zero; FixedVector3 vertex; for (int i = 0; i < planeModel.offsets.Count; ++i) { vertex = planeModel.GetPointFromOffsetId(i); vec2 = vertex - intersection; anglesSum += FixedVector3.Angle(vec1, vec2); vec1 = vec2; } // last vertex with origin anglesSum += FixedVector3.Angle(vec2, originVector); // a small delta due to precision errors return(FixedFloat.Abs(anglesSum - FixedFloat.TwoPI) < 0.2); }