// Natural reaction: slide along the plane
protected bool CollisionNaturalReaction(PhysicWorldModel world, PhysicPointModel pointModel, PhysicPlaneModel planeModel, FixedVector3 intersection)
{
bool intersectionChanged = false;
if (pointModel.position != intersection)
{
// Project remaining velocity against the plane
FixedVector3 normal = planeModel.normal;
FixedVector3 pos1 = pointModel.position;
FixedVector3 pos2 = pointModel.position + normal;
FixedVector3 pointDeltaPos = pos2 - pos1;
FixedVector3 pos1ToOrigin = planeModel.origin - pos1;
FixedFloat dotDeltaPosNormal = FixedVector3.Dot(pointDeltaPos, normal);
FixedFloat t = FixedVector3.Dot(pos1ToOrigin, normal) / dotDeltaPosNormal;
FixedVector3 newIntersection = pos1 + t * pointDeltaPos;
if (newIntersection != intersection)
{
intersection = newIntersection;
intersectionChanged = true;
}
// finally, our new position is the intersection point
pointModel.position = intersection;
}
// in the end, also sum the plane velocity, essential for platforms
GainPlaneVelocity(world, pointModel, planeModel);
return(!intersectionChanged); // if intersection didn't change, it's considered stable
}
public static FixedFloat Angle(FixedVector3 from, FixedVector3 to)
{
if (from.Magnitude == FixedFloat.Zero || to.Magnitude == FixedFloat.Zero)
{
return(FixedFloat.Zero);
}
return(FixedFloat.Acos((FixedVector3.Dot(from, to)) / (from.Magnitude * to.Magnitude)));
}
// 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);
}