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