/// <summary>
/// Constructor
/// </summary>
/// <param name="n"></param>
/// <param name="d"></param>
public Plane(Vector3 n, float d)
: base((int)PrimitiveType.Plane)
{
JiggleMath.NormalizeSafe(ref n);
this.normal = n;
this.d = d;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="n"></param>
/// <param name="pos"></param>
public Plane(Vector3 n, Vector3 pos)
: base((int)PrimitiveType.Plane)
{
JiggleMath.NormalizeSafe(ref n);
this.normal = n;
this.d = -Vector3.Dot(n, pos);
}
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
var info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1)
{
var skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
var primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero;
var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero;
var oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
var newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
var oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
var newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
var oldDist = oldBox.GetDistanceToPoint(out var oldBoxPoint, oldSphere.Position);
var newDist = newBox.GetDistanceToPoint(out var newBoxPoint, newSphere.Position);
var oldDepth = oldSphere.Radius - oldDist;
var newDepth = newSphere.Radius - newDist;
if (System.Math.Max(oldDepth, newDepth) > -collTolerance)
{
Vector3 dir;
if (oldDist < -JiggleMath.Epsilon)
{
dir = oldBoxPoint - oldSphere.Position - oldBoxPoint;
JiggleMath.NormalizeSafe(ref dir);
}
else if (oldDist > JiggleMath.Epsilon)
{
dir = oldSphere.Position - oldBoxPoint;
JiggleMath.NormalizeSafe(ref dir);
}
else
{
dir = oldSphere.Position - oldBox.GetCentre();
JiggleMath.NormalizeSafe(ref dir);
}
unsafe
{
var collInfo = new SmallCollPointInfo(oldBoxPoint - body0Pos, oldBoxPoint - body1Pos, oldDepth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
}
/// <summary>
/// Gets the minimum and maximum extents of the triangle along the axis
/// </summary>
/// <param name="min"></param>
/// <param name="max"></param>
/// <param name="axis"></param>
public void GetSpan(out float min, out float max, Vector3 axis)
{
float d0 = Vector3.Dot(GetPoint(0), axis);
float d1 = Vector3.Dot(GetPoint(1), axis);
float d2 = Vector3.Dot(GetPoint(2), axis);
min = JiggleMath.Min(d0, d1, d2);
max = JiggleMath.Max(d0, d1, d2);
}
/// <summary>
/// UpdateController
/// </summary>
/// <param name="dt"></param>
public override void UpdateController(float dt)
{
if (body0 == null || body1 == null)
{
return;
}
//Assert(0 != mBody0);
//Assert(0 != mBody1);
if (damping > 0.0f)
{
// Some hinges can bend in wonky ways. Derive the effective hinge axis
// using the relative rotation of the bodies.
Vector3 hingeAxis = body1.AngularVelocity - body0.AngularVelocity;
JiggleMath.NormalizeSafe(ref hingeAxis);
float angRot1;//
Vector3.Dot(ref body0.transformRate.AngularVelocity, ref hingeAxis, out angRot1);
float angRot2;
Vector3.Dot(ref body1.transformRate.AngularVelocity, ref hingeAxis, out angRot2);
float avAngRot = 0.5f * (angRot1 + angRot2);
float frac = 1.0f - damping;
float newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
float newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;
Vector3 newAngVel1;// = body0.AngVel + (newAngRot1 - angRot1) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out newAngVel1);
Vector3.Add(ref newAngVel1, ref body0.transformRate.AngularVelocity, out newAngVel1);
Vector3 newAngVel2;// = body1.AngVel + (newAngRot2 - angRot2) * hingeAxis;
Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out newAngVel2);
Vector3.Add(ref newAngVel2, ref body1.transformRate.AngularVelocity, out newAngVel2);
body0.AngularVelocity = newAngVel1;
body1.AngularVelocity = newAngVel2;
}
// the extra torque
if (extraTorque != 0.0f)
{
Vector3 torque1;// = extraTorque * Vector3.Transform(hingeAxis, body0.Orientation);
Vector3.TransformNormal(ref hingeAxis, ref body0.transform.Orientation, out torque1);
Vector3.Multiply(ref torque1, extraTorque, out torque1);
body0.AddWorldTorque(torque1);
body1.AddWorldTorque(-torque1);
}
}
// BEN-OPTIMISATION: New method, ref axis
/// <summary>
/// Gets the minimum and maximum extents of the triangle along the axis
/// </summary>
/// <param name="min"></param>
/// <param name="max"></param>
/// <param name="axis"></param>
public void GetSpan(out float min, out float max, ref Vector3 axis)
{
Vector3 point = new Vector3();
GetPoint(ref point, 0);
float d0 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
GetPoint(ref point, 1);
float d1 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
GetPoint(ref point, 2);
float d2 = point.X * axis.X + point.Y * axis.Y + point.Z * axis.Z;
min = JiggleMath.Min(d0, d1, d2);
max = JiggleMath.Max(d0, d1, d2);
}
/// <summary>
/// SegmentIntersect
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
bool result = Intersection.SegmentCapsuleIntersection(out frac, seg, this);
if (result)
{
pos = seg.GetPoint(frac);
normal = pos - transform.Position;
normal -= Vector3.Dot(normal, transform.Orientation.Backward()) * transform.Orientation.Backward();
JiggleMath.NormalizeSafe(ref normal);
}
else
{
pos = normal = Vector3.Zero;
}
return(result);
}
public override void UpdateController(float dt)
{
if (body0 == null || body1 == null)
{
return;
}
if (damping > 0.0f)
{
var hingeAxis = body1.AngularVelocity - body0.AngularVelocity;
JiggleMath.NormalizeSafe(ref hingeAxis);
Vector3.Dot(ref body0.TransformRate.AngularVelocity, ref hingeAxis, out var angRot1);
Vector3.Dot(ref body1.TransformRate.AngularVelocity, ref hingeAxis, out var angRot2);
var avAngRot = 0.5f * (angRot1 + angRot2);
var frac = 1.0f - damping;
var newAngRot1 = avAngRot + (angRot1 - avAngRot) * frac;
var newAngRot2 = avAngRot + (angRot2 - avAngRot) * frac;
Vector3.Multiply(ref hingeAxis, newAngRot1 - angRot1, out var newAngVel1);
Vector3.Add(ref newAngVel1, ref body0.TransformRate.AngularVelocity, out newAngVel1);
Vector3.Multiply(ref hingeAxis, newAngRot2 - angRot2, out var newAngVel2);
Vector3.Add(ref newAngVel2, ref body1.TransformRate.AngularVelocity, out newAngVel2);
body0.AngularVelocity = newAngVel1;
body1.AngularVelocity = newAngVel2;
}
if (extraTorque != 0.0f)
{
Vector3.TransformNormal(ref hingeAxis, ref body0.Transform.Orientation, out var torque1);
Vector3.Multiply(ref torque1, extraTorque, out torque1);
body0.AddWorldTorque(torque1);
body1.AddWorldTorque(-torque1);
}
}
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
bool result;
result = Intersection.SegmentSphereIntersection(out frac, seg, this);
if (result)
{
pos = seg.GetPoint(frac);
normal = pos - this.transform.Position;
JiggleMath.NormalizeSafe(ref normal);
}
else
{
pos = Vector3.Zero;
normal = Vector3.Zero;
}
return(result);
}
/// <summary>
/// CalcGridForSkin
/// </summary>
/// <param name="i"></param>
/// <param name="j"></param>
/// <param name="k"></param>
/// <param name="skin"></param>
private void CalcGridForSkin(out int i, out int j, out int k, CollisionSkin skin)
{
Vector3 sides = skin.WorldBoundingBox.Max - skin.WorldBoundingBox.Min;
if ((sides.X > dx) || (sides.Y > dy) || (sides.Z > dz))
{
System.Diagnostics.Debug.WriteLine("CollisionSkin too big for gridding system - putting it into overflow list.");
i = j = k = -1;
return;
}
Vector3 min = skin.WorldBoundingBox.Min;
min.X = JiggleMath.Wrap(min.X, 0.0f, sizeX);
min.Y = JiggleMath.Wrap(min.Y, 0.0f, sizeY);
min.Z = JiggleMath.Wrap(min.Z, 0.0f, sizeZ);
i = (int)(min.X / dx) % nx;
j = (int)(min.Y / dy) % ny;
k = (int)(min.Z / dz) % nz;
}
public static bool SweptSphereTriangleIntersection(out Vector3 pt, out Vector3 N, out float depth,
BoundingSphere oldSphere, BoundingSphere newSphere, Triangle triangle,
float oldCentreDistToPlane, float newCentreDistToPlane,
EdgesToTest edgesToTest, CornersToTest cornersToTest)
{
int i;
Microsoft.Xna.Framework.Plane trianglePlane = triangle.Plane;
N = Vector3.Zero;
// Check against plane
if (!SweptSpherePlaneIntersection(out pt, out depth, oldSphere, newSphere, trianglePlane.Normal, oldCentreDistToPlane, newCentreDistToPlane))
{
return(false);
}
Vector3 v0 = triangle.GetPoint(0);
Vector3 v1 = triangle.GetPoint(1);
Vector3 v2 = triangle.GetPoint(2);
Vector3 e0 = v1 - v0;
Vector3 e1 = v2 - v1;
Vector3 e2 = v0 - v2;
// If the point is inside the triangle, this is a hit
bool allInside = true;
Vector3 outDir0 = Vector3.Cross(e0, trianglePlane.Normal);
if (Vector3.Dot(pt - v0, outDir0) > 0.0f)
{
allInside = false;
}
Vector3 outDir1 = Vector3.Cross(e1, trianglePlane.Normal);
if (Vector3.Dot(pt - v1, outDir1) > 0.0f)
{
allInside = false;
}
Vector3 outDir2 = Vector3.Cross(e2, trianglePlane.Normal);
if (Vector3.Dot(pt - v2, outDir2) > 0.0f)
{
allInside = false;
}
// Quick result?
if (allInside)
{
N = trianglePlane.Normal;
return(true);
}
// Now check against the edges
float bestT = float.MaxValue;
Vector3 Ks = newSphere.Center - oldSphere.Center;
float kss = Vector3.Dot(Ks, Ks);
float radius = newSphere.Radius;
float radiusSq = radius * radius;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & ((int)edgesToTest != 0))) // TODO: CHECK THIS
{
continue;
}
Vector3 Ke;
Vector3 vp;
switch (i)
{
case 0:
Ke = e0;
vp = v0;
break;
case 1:
Ke = e1;
vp = v1;
break;
case 2:
default:
Ke = e2;
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
{
continue;
}
float kes = Vector3.Dot(Ke, Ks);
float kgs = Vector3.Dot(Kg, Ks);
float keg = Vector3.Dot(Ke, Kg);
float kgg = Vector3.Dot(Kg, Kg);
// a * t^2 + b * t + c = 0
float a = kee * kss - (kes * kes);
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = 2.0f * (keg * kes - kee * kgs);
float c = kee * (kgg - radiusSq) - keg * keg;
float blah = b * b - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
// now check where it hit on the edge
Vector3 Ct = oldSphere.Center + t * Ks;
float d = Vector3.Dot((Ct - vp), Ke) / kee;
if (d < 0.0f || d > 1.0f)
{
continue;
}
// wahay - got hit. Already checked that t < bestT
bestT = t;
pt = vp + d * Ke;
N = (Ct - pt);// .GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
// depth is already calculated
}
if (bestT <= 1.0f)
{
return(true);
}
// check the corners
bestT = float.MaxValue;
for (i = 0; i < 3; ++i)
{
int mask = 1 << i;
if (!((mask != 0) & (cornersToTest != 0))) // CHECK THIS
{
continue;
}
Vector3 vp;
switch (i)
{
case 0:
vp = v0;
break;
case 1:
vp = v1;
break;
case 2:
default:
vp = v2;
break;
}
Vector3 Kg = vp - oldSphere.Center;
float kgs = Vector3.Dot(Kg, Ks);
float kgg = Vector3.Dot(Kg, Kg);
float a = kss;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
{
continue;
}
float b = -2.0f * kgs;
float c = kgg - radiusSq;
float blah = (b * b) - 4.0f * a * c;
if (blah < 0.0f)
{
continue;
}
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt(blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
{
continue;
}
if (t > bestT)
{
continue;
}
bestT = t;
Vector3 Ct = oldSphere.Center + t * Ks;
N = (Ct - vp);//.GetNormalisedSafe();
JiggleMath.NormalizeSafe(ref N);
}
if (bestT <= 1.0f)
{
return(true);
}
return(false);
}
/// <summary>
/// CollDetectSphereStaticMeshOverlap
/// </summary>
/// <param name="oldSphere"></param>
/// <param name="newSphere"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public static void CollDetectSphereStaticMeshOverlap(BoundingSphere oldSphere, BoundingSphere newSphere,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float sphereTolR = collTolerance + newSphere.Radius;
float sphereTolR2 = sphereTolR * sphereTolR;
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
int[] potTriArray = IntStackAlloc();
int numCollPts = 0;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSphere(newSphere, ref bb);
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potTriArray, MaxLocalStackTris, ref bb);
// Deano : get the spheres centers in triangle mesh space
Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix);
Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potTriArray[iTriangle]);
float distToCentre = meshTriangle.Plane.DotCoordinate(newSphereCen);
// BEN-BUG-FIX: Replaced 0.0f with -sphereTolR.
if (distToCentre < -sphereTolR || distToCentre > sphereTolR)
{
continue;
}
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2));
float s, t;
float newD2 = Distance.PointTriangleDistanceSq(out s, out t, newSphereCen, triangle);
if (newD2 < sphereTolR2)
{
// have overlap - but actually report the old intersection
float oldD2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle);
float dist = (float)System.Math.Sqrt((float)oldD2);
float depth = oldSphere.Radius - dist;
Vector3 triPointSTNorm = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref triPointSTNorm);
Vector3 collisionN = (dist > float.Epsilon) ? triPointSTNorm : triangle.Normal;
// since impulse get applied at the old position
Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reuse existing collPts.
collPtArray[numCollPts].R0 = pt - body0Pos;
collPtArray[numCollPts].R1 = pt - body1Pos;
collPtArray[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPtArray, numCollPts);
}
FreeStackAlloc(potTriArray);
FreeStackAlloc(collPtArray);
}
/// <summary>
/// CollDetectSphereStaticMeshSweep
/// </summary>
/// <param name="oldSphere"></param>
/// <param name="newSphere"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
internal static void CollDetectSphereStaticMeshSweep(BoundingSphere oldSphere, BoundingSphere newSphere, TriangleMesh mesh,
CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
// really use a swept test - or overlap?
Vector3 delta = newSphere.Center - oldSphere.Center;
if (delta.LengthSquared() < (0.25f * newSphere.Radius * newSphere.Radius))
{
CollDetectSphereStaticMeshOverlap(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor);
}
else
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float sphereTolR = collTolerance + oldSphere.Radius;
float sphereToR2 = sphereTolR * sphereTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSphere(oldSphere, ref bb);
BoundingBoxHelper.AddSphere(newSphere, ref bb);
// get the spheres centers in triangle mesh space
Vector3 newSphereCen = Vector3.Transform(newSphere.Center, mesh.InverseTransformMatrix);
Vector3 oldSphereCen = Vector3.Transform(oldSphere.Center, mesh.InverseTransformMatrix);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int *potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed(int *potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
// first test the old sphere for being on the wrong side
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
float distToCentreOld = meshTriangle.Plane.DotCoordinate(oldSphereCen);
if (distToCentreOld <= 0.0f)
{
continue;
}
// now test the new sphere for being clear
float distToCentreNew = meshTriangle.Plane.DotCoordinate(newSphereCen);
if (distToCentreNew > sphereTolR)
{
continue;
}
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Triangle triangle = new Triangle(mesh.GetVertex(i0), mesh.GetVertex(i1), mesh.GetVertex(i2));
// If the old sphere is intersecting, just use that result
float s, t;
float d2 = Distance.PointTriangleDistanceSq(out s, out t, oldSphereCen, triangle);
if (d2 < sphereToR2)
{
float dist = (float)System.Math.Sqrt(d2);
float depth = oldSphere.Radius - dist;
Vector3 triangleN = triangle.Normal;
Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref normSafe);
Vector3 collisionN = (dist > float.Epsilon) ? normSafe : triangleN;
// since impulse gets applied at the old position
Vector3 pt = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reuse existing collPts.
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
else if (distToCentreNew < distToCentreOld)
{
// old sphere is not intersecting - do a sweep, but only if the sphere is moving into the
// triangle
Vector3 pt, N; // CHECK THIS
float depth;
if (Intersection.SweptSphereTriangleIntersection(out pt, out N, out depth, oldSphere, newSphere, triangle,
distToCentreOld, distToCentreNew, Intersection.EdgesToTest.EdgeAll, Intersection.CornersToTest.CornerAll))
{
// collision point etc must be relative to the old position because that's
//where the impulses are applied
float dist = (float)System.Math.Sqrt(d2);
float depth2 = oldSphere.Radius - dist;
Vector3 triangleN = triangle.Normal;
Vector3 normSafe = oldSphereCen - triangle.GetPoint(s, t);
JiggleMath.NormalizeSafe(ref normSafe);
Vector3 collisionN = (dist > JiggleMath.Epsilon) ? normSafe : triangleN;
// since impulse gets applied at the old position
Vector3 pt2 = oldSphere.Center - oldSphere.Radius * collisionN;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reuse existing collPts.
collPts[numCollPts].R0 = pt2 - body0Pos;
collPts[numCollPts].R1 = pt2 - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
}
#if USE_STACKALLOC
}
}
#else
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
}
#endif
}
}
public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point)
{
var x = point.X;
var z = point.Z;
x = MathHelper.Clamp(x, xMin, xMax);
z = MathHelper.Clamp(z, zMin, zMax);
var i0 = (int)((x - xMin) / dx);
var j0 = (int)((point.Z - zMin) / dz);
i0 = MathHelper.Clamp(i0, 0, Heights.Nx - 1);
j0 = MathHelper.Clamp(j0, 0, Heights.Nz - 1);
var i1 = i0 + 1;
var j1 = j0 + 1;
if (i1 >= Heights.Nx)
{
i1 = Heights.Nx - 1;
}
if (j1 >= Heights.Nz)
{
j1 = Heights.Nz - 1;
}
var iFrac = (x - (i0 * dx + xMin)) / dx;
var jFrac = (z - (j0 * dz + zMin)) / dz;
iFrac = MathHelper.Clamp(iFrac, 0.0f, 1.0f);
jFrac = MathHelper.Clamp(jFrac, 0.0f, 1.0f);
var h00 = Heights[i0, j0];
var h01 = Heights[i0, j1];
var h10 = Heights[i1, j0];
var h11 = Heights[i1, j1];
if (i0 == i1 && j0 == j1)
{
normal = Vector3.Up;
}
else if (i0 == i1)
{
var right = Vector3.Right;
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right);
normal.Normalize();
}
if (j0 == j1)
{
var backw = Vector3.Backward;
normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else if (iFrac > jFrac)
{
normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else
{
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz));
normal.Normalize();
}
JiggleMath.NormalizeSafe(ref normal);
var pos = new Vector3(i0 * dx + xMin, h00, j0 * dz + zMin);
Vector3.Dot(ref normal, ref pos, out var d);
d = -d;
h = Distance.PointPlaneDistance(ref point, ref normal, d);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
// get the skins in the order that we're expectiing
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radius = oldCapsule.Radius;
Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
float oldSegT;
float oldBoxT0, oldBoxT1, oldBoxT2;
float oldDistSq = Distance.SegmentBoxDistanceSq(out oldSegT, out oldBoxT0, out oldBoxT1, out oldBoxT2, oldSeg, oldBox);
float newSegT;
float newBoxT0, newBoxT1, newBoxT2;
float newDistSq = Distance.SegmentBoxDistanceSq(out newSegT, out newBoxT0, out newBoxT1, out newBoxT2, newSeg, newBox);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance)))
{
Vector3 segPos = oldSeg.GetPoint(oldSegT);
Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * oldBox.Orientation.Right() +
oldBoxT1 * oldBox.Orientation.Up() + oldBoxT2 * oldBox.Orientation.Backward();
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radius - dist;
Vector3 dir;
if (dist > JiggleMath.Epsilon)
{
dir = segPos - boxPos;
JiggleMath.NormalizeSafe(ref dir);
}
else if ((segPos - oldBox.GetCentre()).LengthSquared > JiggleMath.Epsilon)
{
dir = segPos - oldBox.GetCentre();
JiggleMath.NormalizeSafe(ref dir);
}
else
{
// todo - make this not random
dir = Vector3.Transform(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
}
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
var info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1)
{
var skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
var primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero;
var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero;
var oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
var newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
var oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
var newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
oldBox.GetCornerPoints(out var oldPts);
newBox.GetCornerPoints(out var newPts);
unsafe
{
var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi];
{
var numCollPts = 0;
var collNormal = Vector3.Zero;
for (var i = 0; i < 8; ++i)
{
var newPt = newPts[i];
newHeightmap.GetHeightAndNormal(out var newDist, out var normal, newPt);
if (newDist < collTolerance)
{
var oldPt = oldPts[i];
var oldDist = oldHeightmap.GetHeight(oldPt);
Vector3.Subtract(ref oldPt, ref body0Pos, out var pt0);
Vector3.Subtract(ref oldPt, ref body1Pos, out var pt1);
if (numCollPts < MaxLocalStackScpi)
{
collPts[numCollPts].R0 = pt0;
collPts[numCollPts].R1 = pt1;
collPts[numCollPts++].InitialPenetration = -oldDist;
}
Vector3.Add(ref collNormal, ref normal, out collNormal);
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
}
}
}
/// <summary> // TODO teting testing testing ...
/// Adds the forces die to this wheel to the parent. Return value indicates if it's
/// on the ground.
/// </summary>
/// <param name="dt"></param>
public bool AddForcesToCar(float dt)
{
Vector3 force = Vector3.Zero;
lastDisplacement = displacement;
displacement = 0.0f;
Body carBody = car.Chassis.Body;
Vector3 worldPos = carBody.Position + Vector3.Transform(pos, carBody.Orientation); // *mPos;
Vector3 worldAxis = Vector3.Transform(axisUp, carBody.Orientation); // *mAxisUp;
//Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.Orientation.GetCol(0);
// OpenGl has differnet row/column order for matrixes than XNA has ..
Vector3 wheelFwd = Vector3.Transform(carBody.Orientation.Right, JiggleMath.RotationMatrix(steerAngle, worldAxis));
//Vector3 wheelFwd = RotationMatrix(mSteerAngle, worldAxis) * carBody.GetOrientation().GetCol(0);
Vector3 wheelUp = worldAxis;
Vector3 wheelLeft = Vector3.Cross(wheelUp, wheelFwd);
wheelLeft.Normalize();
wheelUp = Vector3.Cross(wheelFwd, wheelLeft);
// start of ray
float rayLen = 2.0f * radius + travel;
Vector3 wheelRayEnd = worldPos - radius * worldAxis;
Segment wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis);
//Assert(PhysicsSystem.CurrentPhysicsSystem);
CollisionSystem collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem;
///Assert(collSystem);
int numRaysUse = System.Math.Min(numRays, maxNumRays);
// adjust the start position of the ray - divide the wheel into numRays+2
// rays, but don't use the first/last.
float deltaFwd = (2.0f * radius) / (numRaysUse + 1);
float deltaFwdStart = deltaFwd;
lastOnFloor = false;
int bestIRay = 0;
int iRay;
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
fracs[iRay] = float.MaxValue; //SCALAR_HUGE;
// work out the offset relative to the middle ray
float distFwd = (deltaFwdStart + iRay * deltaFwd) - radius;
//float zOffset = mRadius * (1.0f - CosDeg(90.0f * (distFwd / mRadius)));
float zOffset = radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / radius))));
segments[iRay] = wheelRay;
segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp;
if (collSystem.SegmentIntersect(out fracs[iRay], out otherSkins[iRay],
out groundPositions[iRay], out groundNormals[iRay], segments[iRay], pred))
{
lastOnFloor = true;
if (fracs[iRay] < fracs[bestIRay])
{
bestIRay = iRay;
}
}
}
if (!lastOnFloor)
{
return(false);
}
//Assert(bestIRay < numRays);
// use the best one
Vector3 groundPos = groundPositions[bestIRay];
float frac = fracs[bestIRay];
CollisionSkin otherSkin = otherSkins[bestIRay];
// const Vector3 groundNormal = (worldPos - segments[bestIRay].GetEnd()).NormaliseSafe();
// const Vector3 groundNormal = groundNormals[bestIRay];
Vector3 groundNormal = worldAxis;
if (numRaysUse > 1)
{
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
if (fracs[iRay] <= 1.0f)
{
groundNormal += (1.0f - fracs[iRay]) * (worldPos - segments[iRay].GetEnd());
}
}
JiggleMath.NormalizeSafe(ref groundNormal);
}
else
{
groundNormal = groundNormals[bestIRay];
}
//Assert(otherSkin);
Body worldBody = otherSkin.Owner;
displacement = rayLen * (1.0f - frac);
displacement = MathHelper.Clamp(displacement, 0, travel);
float displacementForceMag = displacement * spring;
// reduce force when suspension is par to ground
displacementForceMag *= Vector3.Dot(groundNormals[bestIRay], worldAxis);
// apply damping
float dampingForceMag = upSpeed * damping;
float totalForceMag = displacementForceMag + dampingForceMag;
if (totalForceMag < 0.0f)
{
totalForceMag = 0.0f;
}
Vector3 extraForce = totalForceMag * worldAxis;
force += extraForce;
// side-slip friction and drive force. Work out wheel- and floor-relative coordinate frame
Vector3 groundUp = groundNormal;
Vector3 groundLeft = Vector3.Cross(groundNormal, wheelFwd);
JiggleMath.NormalizeSafe(ref groundLeft);
Vector3 groundFwd = Vector3.Cross(groundLeft, groundUp);
Vector3 wheelPointVel = carBody.Velocity +
Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos);
Vector3 rimVel = angVel * Vector3.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
// if sitting on another body then adjust for its velocity.
if (worldBody != null)
{
Vector3 worldVel = worldBody.Velocity +
Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
wheelPointVel -= worldVel;
}
// sideways forces
float noslipVel = 0.2f;
float slipVel = 0.4f;
float slipFactor = 0.7f;
float smallVel = 3;
float friction = sideFriction;
float sideVel = Vector3.Dot(wheelPointVel, groundLeft);
if ((sideVel > slipVel) || (sideVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((sideVel > noslipVel) || (sideVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel);
}
if (sideVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(sideVel) < smallVel)
{
friction *= System.Math.Abs(sideVel) / smallVel;
}
float sideForce = -friction * totalForceMag;
extraForce = sideForce * groundLeft;
force += extraForce;
// fwd/back forces
friction = fwdFriction;
float fwdVel = Vector3.Dot(wheelPointVel, groundFwd);
if ((fwdVel > slipVel) || (fwdVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((fwdVel > noslipVel) || (fwdVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel);
}
if (fwdVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(fwdVel) < smallVel)
{
friction *= System.Math.Abs(fwdVel) / smallVel;
}
float fwdForce = -friction * totalForceMag;
extraForce = fwdForce * groundFwd;
force += extraForce;
//if (!force.IsSensible())
//{
// TRACE_FILE_IF(ONCE_1)
// TRACE("Bad force in car wheel\n");
// return true;
//}
// fwd force also spins the wheel
Vector3 wheelCentreVel = carBody.Velocity +
Vector3.Cross(carBody.AngularVelocity, Vector3.Transform(pos, carBody.Orientation));// * mPos);
angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / radius;
torque += -fwdForce * radius;
// add force to car
carBody.AddWorldForce(force, groundPos);
//if (float.IsNaN(force.X))
// while(true){}
//System.Diagnostics.Debug.WriteLine(force.ToString());
// add force to the world
if (worldBody != null && !worldBody.Immovable)
{
// todo get the position in the right place...
// also limit the velocity that this force can produce by looking at the
// mass/inertia of the other object
float maxOtherBodyAcc = 500.0f;
float maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass;
if (force.LengthSquared() > (maxOtherBodyForce * maxOtherBodyForce))
{
force *= maxOtherBodyForce / force.Length();
}
worldBody.AddWorldForce(-force, groundPos);
}
return(true);
}
public bool AddForcesToCar(float dt)
{
var force = Vector3.Zero;
_lastDisplacement = Displacement;
Displacement = 0.0f;
var carBody = _car.Chassis.Body;
var worldPos = carBody.Position + Vector3.TransformNormal(Pos, carBody.Orientation);
var worldAxis = Vector3.TransformNormal(LocalAxisUp, carBody.Orientation);
var wheelFwd = Vector3.TransformNormal(carBody.Orientation.Right, JiggleMath.RotationMatrix(SteerAngle, worldAxis));
var wheelUp = worldAxis;
var wheelLeft = Vector3.Cross(wheelUp, wheelFwd);
wheelLeft.Normalize();
wheelUp = Vector3.Cross(wheelFwd, wheelLeft);
var rayLen = 2.0f * Radius + _travel;
var wheelRayEnd = worldPos - Radius * worldAxis;
var wheelRay = new Segment(wheelRayEnd + rayLen * worldAxis, -rayLen * worldAxis);
var collSystem = PhysicsSystem.CurrentPhysicsSystem.CollisionSystem;
var numRaysUse = System.Math.Min(_numRays, MaxNumRays);
var deltaFwd = 2.0f * Radius / (numRaysUse + 1);
var deltaFwdStart = deltaFwd;
OnFloor = false;
var bestIRay = 0;
int iRay;
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
_fracs[iRay] = float.MaxValue;
var distFwd = deltaFwdStart + iRay * deltaFwd - Radius;
var zOffset = Radius * (1.0f - (float)System.Math.Cos(MathHelper.ToRadians(90.0f * (distFwd / Radius))));
_segments[iRay] = wheelRay;
_segments[iRay].Origin += distFwd * wheelFwd + zOffset * wheelUp;
if (collSystem.SegmentIntersect(out _fracs[iRay], out _otherSkins[iRay], out _groundPositions[iRay], out _groundNormals[iRay], _segments[iRay], _pred))
{
OnFloor = true;
if (_fracs[iRay] < _fracs[bestIRay])
{
bestIRay = iRay;
}
}
}
if (!OnFloor)
{
return(false);
}
var groundPos = _groundPositions[bestIRay];
var frac = _fracs[bestIRay];
var otherSkin = _otherSkins[bestIRay];
var groundNormal = worldAxis;
if (numRaysUse > 1)
{
for (iRay = 0; iRay < numRaysUse; ++iRay)
{
if (_fracs[iRay] <= 1.0f)
{
groundNormal += (1.0f - _fracs[iRay]) * (worldPos - _segments[iRay].GetEnd());
}
}
JiggleMath.NormalizeSafe(ref groundNormal);
}
else
{
groundNormal = _groundNormals[bestIRay];
}
var worldBody = otherSkin.Owner;
Displacement = rayLen * (1.0f - frac);
Displacement = MathHelper.Clamp(Displacement, 0, _travel);
var displacementForceMag = Displacement * _spring;
displacementForceMag *= Vector3.Dot(_groundNormals[bestIRay], worldAxis);
var dampingForceMag = _upSpeed * _damping;
var totalForceMag = displacementForceMag + dampingForceMag;
if (totalForceMag < 0.0f)
{
totalForceMag = 0.0f;
}
var extraForce = totalForceMag * worldAxis;
force += extraForce;
var groundUp = groundNormal;
var groundLeft = Vector3.Cross(groundNormal, wheelFwd);
JiggleMath.NormalizeSafe(ref groundLeft);
var groundFwd = Vector3.Cross(groundLeft, groundUp);
var wheelPointVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation));
var rimVel = _angVel * Vector3.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
if (worldBody != null)
{
var worldVel = worldBody.Velocity + Vector3.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
wheelPointVel -= worldVel;
}
var noslipVel = 0.2f;
var slipVel = 0.4f;
var slipFactor = 0.7f;
float smallVel = 3;
var friction = _sideFriction;
var sideVel = Vector3.Dot(wheelPointVel, groundLeft);
if (sideVel > slipVel || sideVel < -slipVel)
{
friction *= slipFactor;
}
else if (sideVel > noslipVel || sideVel < -noslipVel)
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel);
}
if (sideVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(sideVel) < smallVel)
{
friction *= System.Math.Abs(sideVel) / smallVel;
}
var sideForce = -friction * totalForceMag;
extraForce = sideForce * groundLeft;
force += extraForce;
friction = _fwdFriction;
var fwdVel = Vector3.Dot(wheelPointVel, groundFwd);
if (fwdVel > slipVel || fwdVel < -slipVel)
{
friction *= slipFactor;
}
else if (fwdVel > noslipVel || fwdVel < -noslipVel)
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel);
}
if (fwdVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(fwdVel) < smallVel)
{
friction *= System.Math.Abs(fwdVel) / smallVel;
}
var fwdForce = -friction * totalForceMag;
extraForce = fwdForce * groundFwd;
force += extraForce;
var wheelCentreVel = carBody.Velocity + Vector3.Cross(carBody.AngularVelocity, Vector3.TransformNormal(Pos, carBody.Orientation));
_angVelForGrip = Vector3.Dot(wheelCentreVel, groundFwd) / Radius;
_torque += -fwdForce * Radius;
carBody.AddWorldForce(force, groundPos);
if (worldBody != null && !worldBody.Immovable)
{
var maxOtherBodyAcc = 500.0f;
var maxOtherBodyForce = maxOtherBodyAcc * worldBody.Mass;
if (force.LengthSquared() > maxOtherBodyForce * maxOtherBodyForce)
{
force *= maxOtherBodyForce / force.Length();
}
worldBody.AddWorldForce(-force, groundPos);
}
return(true);
}
/// <summary>
/// The AABox has a corner at the origin and size sides.
/// </summary>
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts,
ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1,
ref Matrix origBoxOrient, ref Vector3 origBoxPos,
float combinationDistanceSq)
{
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests. We know that the
// AABox has a corner at the origin
#region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0);
Vector3 edgeDir;
Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
#endregion
int num = 0;
for (int idir = 3; idir-- != 0;)
{
// skip edge/face tests if nearly parallel
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
int jdir = (idir + 1) % 3;
int kdir = (idir + 2) % 3;
for (int iface = 2; iface-- != 0;)
{
float offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
#region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1
Vector3 tmp; Vector3 pt;
Vector3.Multiply(ref edgePt1, frac, out tmp);
Vector3.Multiply(ref edgePt0, 1.0f - frac, out pt);
Vector3.Add(ref pt, ref tmp, out pt);
#endregion
// check the point is within the face rectangle
float ptJdir = JiggleUnsafe.Get(ref pt, jdir);
float ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if ((ptJdir > -JiggleMath.Epsilon) &&
(ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) &&
(ptKdir > -JiggleMath.Epsilon) &&
(ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon))
{
// woohoo got a point
#region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient);
Vector3 pos;
Vector3.Transform(ref pt, ref origBoxOrient, out pos);
Vector3.Add(ref origBoxPos, ref pos, out pos);
#endregion
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
Capsule newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
int numCollPts = 0;
Vector3 averageNormal = Vector3.Zero;
// the start
{
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPtArray[numCollPts].R0 = worldPos - body0Pos;
collPtArray[numCollPts].R1 = worldPos - body1Pos;
collPtArray[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
// the end
{
Vector3 oldEnd = oldCapsule.GetEnd();
Vector3 newEnd = newCapsule.GetEnd();
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldEnd - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPtArray[numCollPts].R0 = worldPos - body0Pos;
collPtArray[numCollPts].R1 = worldPos - body1Pos;
collPtArray[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref averageNormal);
collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPtArray, numCollPts);
}
FreeStackAlloc(collPtArray);
}
/// <summary>
/// Detect BoxHeightmap Collisions.
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
Heightmap oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
Heightmap newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
Vector3[] oldPts, newPts;
oldBox.GetCornerPoints(out oldPts);
newBox.GetCornerPoints(out newPts);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 collNormal = Vector3.Zero;
for (int i = 0; i < 8; ++i)
{
Vector3 newPt = newPts[i];
float newDist;
Vector3 normal;
newHeightmap.GetHeightAndNormal(out newDist, out normal, newPt);
if (newDist < collTolerance)
{
Vector3 oldPt = oldPts[i];
float oldDist = oldHeightmap.GetHeight(oldPt);
#region REFERENCE: collPts.Add(new CollPointInfo(oldPt - body0Pos, oldPt - body1Pos, -oldDist));
Vector3 pt0;
Vector3 pt1;
Vector3.Subtract(ref oldPt, ref body0Pos, out pt0);
Vector3.Subtract(ref oldPt, ref body1Pos, out pt1);
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPts[numCollPts].R0 = pt0;
collPts[numCollPts].R1 = pt1;
collPts[numCollPts++].InitialPenetration = -oldDist;
}
#endregion
#region REFERENCE: collNormal += normal;
Vector3.Add(ref collNormal, ref normal, out collNormal);
#endregion
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
var info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == Type1)
{
var skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
var primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero;
var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero;
var oldCapsule = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Capsule;
var newCapsule = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Capsule;
var oldHeightmap = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Heightmap;
var newHeightmap = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Heightmap;
unsafe
{
var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi];
{
var numCollPts = 0;
var averageNormal = Vector3.Zero;
{
oldHeightmap.GetHeightAndNormal(out var oldDist, out var normal, oldCapsule.Position);
newHeightmap.GetHeightAndNormal(out var newDist, out normal, newCapsule.Position);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
var oldDepth = oldCapsule.Radius - oldDist;
var worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackScpi)
{
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
{
var oldEnd = oldCapsule.GetEnd();
var newEnd = newCapsule.GetEnd();
oldHeightmap.GetHeightAndNormal(out var oldDist, out var normal, oldEnd);
newHeightmap.GetHeightAndNormal(out var newDist, out normal, newEnd);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
var oldDepth = oldCapsule.Radius - oldDist;
var worldPos = oldEnd - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackScpi)
{
collPts[numCollPts].R0 = worldPos - body0Pos;
collPts[numCollPts].R1 = worldPos - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
averageNormal += normal;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref averageNormal);
collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts);
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
Heightmap oldHeightmap = (Heightmap)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
Heightmap newHeightmap = (Heightmap)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
Vector3 averageNormal = Vector3.Zero;
// the start
{
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldCapsule.Position);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newCapsule.Position);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
}
averageNormal += normal;
}
}
// the end
{
Vector3 oldEnd = oldCapsule.GetEnd();
Vector3 newEnd = newCapsule.GetEnd();
float oldDist, newDist;
Vector3 normal;
oldHeightmap.GetHeightAndNormal(out oldDist, out normal, oldEnd);
newHeightmap.GetHeightAndNormal(out newDist, out normal, newEnd);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldEnd - oldCapsule.Radius * normal;
if (numCollPts < MaxLocalStackSCPI)
{
collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
}
averageNormal += normal;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref averageNormal);
collisionFunctor.CollisionNotify(ref info, ref averageNormal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts, ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1, ref Matrix origBoxOrient, ref Vector3 origBoxPos, float combinationDistanceSq)
{
Vector3.Subtract(ref edgePt1, ref edgePt0, out var edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
var num = 0;
var pt = new Vector3();
for (var idir = 3; idir-- != 0;)
{
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
var jdir = (idir + 1) % 3;
var kdir = (idir + 2) % 3;
for (var iface = 2; iface-- != 0;)
{
var offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
var dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
var dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
var frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
var tempFrac = 1.0f - frac;
pt.X = tempFrac * edgePt0.X + frac * edgePt1.X;
pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y;
pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z;
var ptJdir = JiggleUnsafe.Get(ref pt, jdir);
var ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if (ptJdir > -JiggleMath.Epsilon && ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon && ptKdir > -JiggleMath.Epsilon && ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon)
{
Vector3.TransformNormal(ref pt, ref origBoxOrient, out var pos);
pos.X += origBoxPos.X;
pos.Y += origBoxPos.Y;
pos.Z += origBoxPos.Z;
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
/// <summary>
/// The AABox has a corner at the origin and size sides.
/// </summary>
/// <param name="pts"></param>
/// <param name="sides"></param>
/// <param name="box"></param>
/// <param name="edgePt0"></param>
/// <param name="edgePt1"></param>
/// <param name="origBoxOrient"></param>
/// <param name="origBoxPos"></param>
/// <param name="combinationDistanceSq"></param>
/// <returns>int</returns>
private static int GetAABox2EdgeIntersectionPoints(List <ContactPoint> pts,
ref Vector3 sides, Box box, ref Vector3 edgePt0, ref Vector3 edgePt1,
ref Matrix origBoxOrient, ref Vector3 origBoxPos,
float combinationDistanceSq)
{
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests. We know that the
// AABox has a corner at the origin
#region REFERENCE: Vector3 edgeDir = JiggleMath.NormalizeSafe(edgePt1 - edgePt0);
Vector3 edgeDir;
Vector3.Subtract(ref edgePt1, ref edgePt0, out edgeDir);
JiggleMath.NormalizeSafe(ref edgeDir);
#endregion
int num = 0;
// BEN-OPTIMISATION: Reuse the one Vector3
Vector3 pt = new Vector3();
for (int idir = 3; idir-- != 0;)
{
// skip edge/face tests if nearly parallel
if (System.Math.Abs(JiggleUnsafe.Get(ref edgeDir, idir)) < 0.1f)
{
continue;
}
int jdir = (idir + 1) % 3;
int kdir = (idir + 2) % 3;
for (int iface = 2; iface-- != 0;)
{
float offset = 0.0f;
if (iface == 1)
{
offset = JiggleUnsafe.Get(ref sides, idir);
}
float dist0 = JiggleUnsafe.Get(ref edgePt0, idir) - offset;
float dist1 = JiggleUnsafe.Get(ref edgePt1, idir) - offset;
float frac = -1.0f;
if (dist0 * dist1 < -JiggleMath.Epsilon)
{
frac = -dist0 / (dist1 - dist0);
}
else if (System.Math.Abs(dist0) < JiggleMath.Epsilon)
{
frac = 0.0f;
}
else if (System.Math.Abs(dist1) < JiggleMath.Epsilon)
{
frac = 1.0f;
}
if (frac >= 0.0f)
{
#region REFERENCE: Vector3 pt = (1.0f - frac) * edgePt0 + frac * edgePt1
// BEN-OPTIMISATION: Reusing pt and inlined.
float tempFrac = 1.0f - frac;
pt.X = tempFrac * edgePt0.X + frac * edgePt1.X;
pt.Y = tempFrac * edgePt0.Y + frac * edgePt1.Y;
pt.Z = tempFrac * edgePt0.Z + frac * edgePt1.Z;
#endregion
// check the point is within the face rectangle
float ptJdir = JiggleUnsafe.Get(ref pt, jdir);
float ptKdir = JiggleUnsafe.Get(ref pt, kdir);
if ((ptJdir > -JiggleMath.Epsilon) &&
(ptJdir < JiggleUnsafe.Get(ref sides, jdir) + JiggleMath.Epsilon) &&
(ptKdir > -JiggleMath.Epsilon) &&
(ptKdir < JiggleUnsafe.Get(ref sides, kdir) + JiggleMath.Epsilon))
{
// woohoo got a point
#region REFERENCE: Vector3 pos = origBoxPos + Vector3.Transform(pt, origBoxOrient);
// BEN-OPTIMISATION: Inlined add because this function can be called very often!
Vector3 pos;
Vector3.TransformNormal(ref pt, ref origBoxOrient, out pos);
pos.X += origBoxPos.X;
pos.Y += origBoxPos.Y;
pos.Z += origBoxPos.Z;
#endregion
AddPoint(pts, ref pos, combinationDistanceSq);
if (++num == 2)
{
return(num);
}
}
}
}
}
return(num);
}
/// <summary>
/// GetHeightAndNormal
/// </summary>
/// <param name="h"></param>
/// <param name="normal"></param>
/// <param name="point"></param>
public void GetHeightAndNormal(out float h, out Vector3 normal, Vector3 point)
{
float x = point.X;
float z = point.Z;
x = OpenTKHelper.Clamp(x, xMin, xMax);
z = OpenTKHelper.Clamp(z, zMin, zMax);
int i0 = (int)((x - xMin) / dx);
int j0 = (int)((point.Z - zMin) / dz);
i0 = (int)OpenTKHelper.Clamp((int)i0, 0, mHeights.Nx - 1);
j0 = (int)OpenTKHelper.Clamp((int)j0, 0, mHeights.Nz - 1);
int i1 = i0 + 1;
int j1 = j0 + 1;
if (i1 >= (int)mHeights.Nx)
{
i1 = mHeights.Nx - 1;
}
if (j1 >= (int)mHeights.Nz)
{
j1 = mHeights.Nz - 1;
}
float iFrac = (x - (i0 * dx + xMin)) / dx;
float jFrac = (z - (j0 * dz + zMin)) / dz;
iFrac = OpenTKHelper.Clamp(iFrac, 0.0f, 1.0f);
jFrac = OpenTKHelper.Clamp(jFrac, 0.0f, 1.0f);
float h00 = mHeights[i0, j0];
float h01 = mHeights[i0, j1];
float h10 = mHeights[i1, j0];
float h11 = mHeights[i1, j1];
// All the triangles are orientated the same way.
// work out the normal, then z is in the plane of this normal
if ((i0 == i1) && (j0 == j1))
{
normal = Vector3Extensions.Up;
}
else if (i0 == i1)
{
Vector3 right = Vector3Extensions.Right;
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), right);
normal.Normalize();
}
if (j0 == j1)
{
Vector3 backw = Vector3Extensions.Backward;
normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else if (iFrac > jFrac)
{
normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else
{
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz));
normal.Normalize();
}
// get the plane equation
// h00 is in all the triangles
JiggleMath.NormalizeSafe(ref normal);
Vector3 pos = new Vector3((i0 * dx + xMin), h00, (j0 * dz + zMin));
float d; Vector3.Dot(ref normal, ref pos, out d); d = -d;
h = Distance.PointPlaneDistance(ref point, ref normal, d);
}
/// <summary>
/// CollDetectCapsuleStaticMeshOverlap
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float capsuleTolR = collTolerance + newCapsule.Radius;
float capsuleTolR2 = capsuleTolR * capsuleTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddCapsule(newCapsule, ref bb);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int *potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed(int *potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
Vector3 capsuleStart = newCapsule.Position;
Vector3 capsuleEnd = newCapsule.GetEnd();
Matrix meshInvTransform = mesh.InverseTransformMatrix;
Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform);
Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
// we do the plane test using the capsule in mesh space
float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart);
float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd);
// BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR.
if ((distToStart > capsuleTolR && distToEnd > capsuleTolR) ||
(distToStart < -capsuleTolR && distToEnd < -capsuleTolR))
{
continue;
}
// we now transform the triangle into world space (we could keep leave the mesh alone
// but at this point 3 vector transforms is probably not a major slow down)
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(i0, out triVec0);
mesh.GetVertex(i1, out triVec1);
mesh.GetVertex(i2, out triVec2);
// Deano move tri into world space
Matrix transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2);
Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart);
float tS, tT0, tT1;
float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle);
if (d2 < capsuleTolR2)
{
Vector3 oldCapsuleStart = oldCapsule.Position;
Vector3 oldCapsuleEnd = oldCapsule.GetEnd();
Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart);
d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle);
// report result from old position
float dist = (float)System.Math.Sqrt(d2);
float depth = oldCapsule.Radius - dist;
Vector3 pt = triangle.GetPoint(tT0, tT1);
Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) :
meshTriangle.Plane.Normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reused existing collPts.
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
#if USE_STACKALLOC
}
}
#else
}
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// CalcGridForSkin
/// </summary>
/// <param name="i"></param>
/// <param name="j"></param>
/// <param name="k"></param>
/// <param name="fi"></param>
/// <param name="fj"></param>
/// <param name="fk"></param>
/// <param name="skin"></param>
public void CalcGridForSkin(out int i, out int j, out int k, out float fi,
out float fj, out float fk, CollisionSkin skin)
{
Vector3 sides = skin.WorldBoundingBox.Max - skin.WorldBoundingBox.Min;
if ((sides.X > dx) || (sides.Y > dy) || (sides.Z > dz))
{
System.Diagnostics.Debug.WriteLine("CollisionSkin too big for gridding system - putting it into overflow list.");
i = j = k = -1;
fi = fj = fk = 0.0f;
return;
}
Vector3 min = skin.WorldBoundingBox.Min;
min.X = JiggleMath.Wrap(min.X, 0.0f, sizeX);
min.Y = JiggleMath.Wrap(min.Y, 0.0f, sizeY);
min.Z = JiggleMath.Wrap(min.Z, 0.0f, sizeZ);
fi = min.X / dx;
fj = min.Y / dy;
fk = min.Z / dz;
i = (int)fi;
j = (int)fj;
k = (int)fk;
if (i < 0)
{
i = 0; fi = 0.0f;
}
else if (i >= (int)nx)
{
i = 0; fi = 0.0f;
}
else
{
fi -= (float)i;
}
if (j < 0)
{
j = 0; fj = 0.0f;
}
else if (j >= (int)ny)
{
j = 0; fj = 0.0f;
}
else
{
fj -= (float)j;
}
if (k < 0)
{
k = 0; fk = 0.0f;
}
else if (k >= (int)nz)
{
k = 0; fk = 0.0f;
}
else
{
fk -= (float)k;
}
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="infoOrig"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo infoOrig, float collTolerance, CollisionFunctor collisionFunctor)
{
// get the skins in the order that we're expecting
CollDetectInfo info = infoOrig;
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper sweep test
Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
Box oldBox = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Box;
Box newBox = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Box;
Vector3 oldBoxPoint;
Vector3 newBoxPoint;
float oldDist = oldBox.GetDistanceToPoint(out oldBoxPoint, oldSphere.Position);
float newDist = newBox.GetDistanceToPoint(out newBoxPoint, newSphere.Position);
// normally point will be outside
float oldDepth = oldSphere.Radius - oldDist;
float newDepth = newSphere.Radius - newDist;
if (System.Math.Max(oldDepth, newDepth) > -collTolerance)
{
Vector3 dir;
if (oldDist < -JiggleMath.Epsilon)
{
dir = oldBoxPoint - oldSphere.Position - oldBoxPoint;
JiggleMath.NormalizeSafe(ref dir);
}
else if (oldDist > JiggleMath.Epsilon)
{
dir = oldSphere.Position - oldBoxPoint;
JiggleMath.NormalizeSafe(ref dir);
}
else
{
dir = oldSphere.Position - oldBox.GetCentre();
JiggleMath.NormalizeSafe(ref dir);
}
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(oldBoxPoint - body0Pos,
oldBoxPoint - body1Pos, oldDepth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
#endregion
}
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule, TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
var body0Pos = info.Skin0.Owner?.OldPosition ?? Vector3.Zero;
var body1Pos = info.Skin1.Owner?.OldPosition ?? Vector3.Zero;
var capsuleTolR = collTolerance + newCapsule.Radius;
var capsuleTolR2 = capsuleTolR * capsuleTolR;
var collNormal = Vector3.Zero;
var bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddCapsule(newCapsule, ref bb);
unsafe
{
var collPts = stackalloc SmallCollPointInfo[MaxLocalStackScpi];
var potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
var numCollPts = 0;
var numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
var capsuleStart = newCapsule.Position;
var capsuleEnd = newCapsule.GetEnd();
var meshInvTransform = mesh.InverseTransformMatrix;
var meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform);
var meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform);
for (var iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
var meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
var distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart);
var distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd);
if (distToStart > capsuleTolR && distToEnd > capsuleTolR || distToStart < -capsuleTolR && distToEnd < -capsuleTolR)
{
continue;
}
meshTriangle.GetVertexIndices(out var i0, out var i1, out var i2);
mesh.GetVertex(i0, out var triVec0);
mesh.GetVertex(i1, out var triVec1);
mesh.GetVertex(i2, out var triVec2);
var transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
var triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2);
var seg = new Segment(capsuleStart, capsuleEnd - capsuleStart);
var d2 = Distance.SegmentTriangleDistanceSq(out var tS, out var tT0, out var tT1, seg, triangle);
if (d2 < capsuleTolR2)
{
var oldCapsuleStart = oldCapsule.Position;
var oldCapsuleEnd = oldCapsule.GetEnd();
var oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart);
d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle);
var dist = (float)System.Math.Sqrt(d2);
var depth = oldCapsule.Radius - dist;
var pt = triangle.GetPoint(tT0, tT1);
var collisionN = d2 > JiggleMath.Epsilon ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) : meshTriangle.Plane.Normal;
if (numCollPts < MaxLocalStackScpi)
{
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
}
}
}
}
