/// <summary>
/// Indicates if a segment intersects a triangle
/// </summary>
/// <param name="seg"></param>
/// <param name="triangle"></param>
/// <returns></returns>
public static bool SegmentTriangleOverlap(Segment seg, Triangle triangle)
{
/// the parameters - if hit then they get copied into the args
float u, v, t;
Vector3 e1 = triangle.Edge0;
Vector3 e2 = triangle.Edge1;
Vector3 p = Vector3.Cross(seg.Delta, e2);
float a = Vector3.Dot(e1, p);
if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon)
return false;
float f = 1.0f / a;
Vector3 s = seg.Origin - triangle.Origin;
u = f * Vector3.Dot(s, p);
if (u < 0.0f || u > 1.0f)
return false;
Vector3 q = Vector3.Cross(s, e1);
v = f * Vector3.Dot(seg.Delta, q);
if (v < 0.0f || (u + v) > 1.0f)
return false;
t = f * Vector3.Dot(e2, q);
if (t < 0.0f || t > 1.0f)
return false;
return true;
}
public override void CollDetect(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;
// todo - proper swept test
#region Get Cylinders
Cylinder oldCylinder0 = (Cylinder)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Cylinder newCylinder0 = (Cylinder)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
Cylinder oldCylinder1 = (Cylinder)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
Cylinder newCylinder1 = (Cylinder)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
#endregion
Segment oldSeg0 = new Segment(oldCylinder0.Position, oldCylinder0.Length * MatrixHelper.GetBackward(oldCylinder0.Orientation));
Segment newSeg0 = new Segment(newCylinder0.Position, newCylinder0.Length * MatrixHelper.GetBackward(newCylinder0.Orientation));
Segment oldSeg1 = new Segment(oldCylinder1.Position, oldCylinder1.Length * MatrixHelper.GetBackward(oldCylinder1.Orientation));
Segment newSeg1 = new Segment(newCylinder1.Position, newCylinder1.Length * MatrixHelper.GetBackward(newCylinder1.Orientation));
float radSum = newCylinder0.Radius + newCylinder1.Radius;
float oldt0, oldt1;
float newt0, newt1;
float oldDistSq = Distance.SegmentSegmentDistanceSq(out oldt0, out oldt1, oldSeg0, oldSeg1);
float newDistSq = Distance.SegmentSegmentDistanceSq(out newt0, out newt1, newSeg0, newSeg1);
if (System.Math.Min(oldDistSq, newDistSq) < ((radSum + collTolerance) * (radSum + collTolerance)))
{
Vector3 pos0 = oldSeg0.GetPoint(oldt0);
Vector3 pos1 = oldSeg1.GetPoint(oldt1);
Vector3 delta = pos0 - pos1;
float dist = (float)System.Math.Sqrt((float)oldDistSq);
float depth = radSum - dist;
if (dist > JiggleMath.Epsilon)
{
delta /= dist;
}
else
{
// todo - make this not random
delta = Vector3.TransformCoordinate(Vector3Helper.Backward, Matrix.RotationAxis(Vector3Helper.Up, MathHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = pos1 +
(oldCylinder1.Radius - 0.5f * depth) * delta;
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1);
}
}
}
/// <summary>
/// Indicates if a segment overlaps an AABox
/// </summary>
/// <param name="seg"></param>
/// <param name="AABox"></param>
/// <returns></returns>
public static bool SegmentAABoxOverlap(Segment seg, AABox AABox)
{
Vector3 p0 = seg.Origin;
Vector3 p1 = seg.GetEnd();
float[] faceOffsets = new float[2];
// The AABox faces are aligned with the world directions. Loop
// over the 3 directions and do the two tests.
for (int iDir = 0; iDir < 3; iDir++)
{
int jDir = (iDir + 1) % 3;
int kDir = (iDir + 2) % 3;
// one plane goes through the origin, one is offset
faceOffsets[0] = JiggleUnsafe.Get(AABox.MinPos, iDir);
faceOffsets[1] = JiggleUnsafe.Get(AABox.MaxPos, iDir);
for (int iFace = 0; iFace < 2; iFace++)
{
// distance of each point from to the face plane
float dist0 = JiggleUnsafe.Get(ref p0, iDir) - faceOffsets[iFace];
float dist1 = JiggleUnsafe.Get(ref p1, iDir) - faceOffsets[iFace];
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)
{
//Assert(frac <= 1.0f);
Vector3 pt = seg.GetPoint(frac);
// check the point is within the face rectangle
if ((JiggleUnsafe.Get(ref pt, jDir) > JiggleUnsafe.Get(AABox.MinPos, jDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, jDir) < JiggleUnsafe.Get(AABox.MaxPos, jDir) + JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) > JiggleUnsafe.Get(AABox.MinPos, kDir) - JiggleMath.Epsilon) &&
(JiggleUnsafe.Get(ref pt, kDir) < JiggleUnsafe.Get(AABox.MaxPos, kDir) + JiggleMath.Epsilon))
{
return true;
}
}
}
}
return false;
}
public static bool SegmentCapsuleIntersection(out float tS, Segment seg, Capsule capsule)
{
float bestFrac = float.MaxValue;
tS = 0;
// do the main sides
float sideFrac = float.MaxValue;
if (!SegmentInfiniteCylinderIntersection(out sideFrac, seg,
new Segment(capsule.Position, MatrixHelper.GetBackward(capsule.Orientation)),
capsule.Radius))
return false; // check this
// only keep this if the side intersection point is within the capsule segment ends
Vector3 sidePos = seg.GetPoint(sideFrac);
if (Vector3.Dot(sidePos - capsule.Position, MatrixHelper.GetBackward(capsule.Orientation)) < 0.0f)
sideFrac = float.MaxValue;
else if (Vector3.Dot(sidePos - capsule.GetEnd(), MatrixHelper.GetBackward(capsule.Orientation)) > 0.0f)
sideFrac = float.MaxValue;
// do the two ends
float originFrac = float.MaxValue;
SegmentSphereIntersection(out originFrac, seg, new Sphere(capsule.Position, capsule.Radius));
float endFrac = float.MaxValue; // Check this!
SegmentSphereIntersection(out endFrac, seg, new Sphere(capsule.GetEnd(), capsule.Radius));
bestFrac = MathHelper.Min(sideFrac, originFrac);
bestFrac = MathHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return true;
}
return false;
}
public static bool SegmentTriangleIntersection(out float tS, out float tT0, out float tT1,
Segment seg, Triangle triangle)
{
/// the parameters - if hit then they get copied into the args
float u, v, t;
tS = 0;
tT0 = 0;
tT1 = 0;
Vector3 e1 = triangle.Edge0;
Vector3 e2 = triangle.Edge1;
Vector3 p = Vector3.Cross(seg.Delta, e2);
float a = Vector3.Dot(e1, p);
if (a > -JiggleMath.Epsilon && a < JiggleMath.Epsilon)
return false;
float f = 1.0f / a;
Vector3 s = seg.Origin - triangle.Origin;
u = f * Vector3.Dot(s, p);
if (u < 0.0f || u > 1.0f)
return false;
Vector3 q = Vector3.Cross(s, e1);
v = f * Vector3.Dot(seg.Delta, q);
if (v < 0.0f || (u + v) > 1.0f)
return false;
t = f * Vector3.Dot(e2, q);
if (t < 0.0f || t > 1.0f)
return false;
tS = t;
tT0 = u;
tT1 = v;
//if (tS != 0) tS = t;
//if (tT0 != 0) tT0 = u;
//if (tT1 != 0) tT1 = v;
return true;
}
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
// move segment into octree space
seg.Origin = Vector3.TransformCoordinate(seg.Origin, invTransform);
seg.Delta = Vector3.TransformNormal(seg.Delta, invTransform);
BoundingBox segBox = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSegment(seg, ref segBox);
unsafe
{
#if USE_STACKALLOC
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
#else
int[] potTriArray = DetectFunctor.IntStackAlloc();
fixed (int* potentialTriangles = potTriArray)
{
#endif
int numTriangles = GetTrianglesIntersectingtAABox(potentialTriangles, DetectFunctor.MaxLocalStackTris, ref segBox);
float tv1, tv2;
pos = Vector3.Zero;
normal = Vector3.Zero;
float bestFrac = float.MaxValue;
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = GetTriangle(potentialTriangles[iTriangle]);
float thisFrac;
Triangle tri = new Triangle(GetVertex(meshTriangle.GetVertexIndex(0)),
GetVertex(meshTriangle.GetVertexIndex(1)),
GetVertex(meshTriangle.GetVertexIndex(2)));
if (Intersection.SegmentTriangleIntersection(out thisFrac, out tv1, out tv2, seg, tri))
{
if (thisFrac < bestFrac)
{
bestFrac = thisFrac;
// re-project
pos = Vector3.TransformCoordinate(seg.GetPoint(thisFrac), transformMatrix);
normal = Vector3.TransformNormal(meshTriangle.Plane.Normal, transformMatrix);
}
}
}
frac = bestFrac;
if (bestFrac < float.MaxValue)
{
DetectFunctor.FreeStackAlloc(potTriArray);
return true;
}
else
{
DetectFunctor.FreeStackAlloc(potTriArray);
return false;
}
#if USE_STACKALLOC
}
#else
}
#endif
}
}
public static bool SegmentPlaneIntersection(out float tS, Segment seg, Plane plane)
{
float denom = Vector3.Dot(plane.Normal, seg.Delta);
if (System.Math.Abs(denom) > JiggleMath.Epsilon)
{
float t = -(Vector3.Dot(plane.Normal, seg.Origin) + plane.D) / denom;
if (t < 0.0f || t > 1.0f)
{
tS = 0.0f;
return false;
}
tS = t;
return true;
}
else
{
// parallel - return false even if it's in the plane
tS = 0.0f;
return false;
}
}
public static bool SegmentSphereIntersection(out float ts, Segment seg, Sphere sphere)
{
Vector3 r = seg.Delta;
Vector3 s = seg.Origin - sphere.Position;
float radiusSq = sphere.Radius * sphere.Radius;
float rSq = r.LengthSquared();
ts = float.MaxValue;
if (rSq < radiusSq)
{
// starting inside
ts = 0.0f;
return false;
}
float sDotr = Vector3.Dot(s, r);
float sSq = s.LengthSquared();
float sigma = (sDotr * sDotr) - rSq * (sSq - radiusSq);
if (sigma < 0.0f)
return false;
float sigmaSqrt = (float)System.Math.Sqrt((float)sigma);
float lambda1 = (-sDotr - sigmaSqrt) / rSq;
float lambda2 = (-sDotr + sigmaSqrt) / rSq;
if (lambda1 > 1.0f || lambda2 < 0.0f)
return false;
// intersection!
ts = MathHelper.Max(lambda1, 0.0f);
return true;
}
public override bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
bool result;
if (result = Intersection.SegmentPlaneIntersection(out frac, seg, this))
{
pos = seg.GetPoint(frac);
normal = this.Normal;
}
else
{
pos = Vector3.Zero;
normal = Vector3.Zero;
}
return result;
}
public static bool SegmentInfiniteCylinderIntersection(out float tS, Segment seg, Segment cylinderAxis, float radius)
{
Vector3 Ks = seg.Delta;
float kss = Vector3.Dot(Ks, Ks);
float radiusSq = radius * radius;
Vector3 Ke = cylinderAxis.Delta;
Vector3 Kg = cylinderAxis.Origin - seg.Origin;
tS = 0.0f;
float kee = Vector3.Dot(Ke, Ke);
if (System.Math.Abs(kee) < JiggleMath.Epsilon)
return false;
float kes = Vector3.Dot(Ke, Ks);
float kgs = Vector3.Dot(Kg, Ks);
float keg = Vector3.Dot(Ke, Kg);
float kgg = Vector3.Dot(Kg, Kg);
// check if start is inside
float distSq = (Kg - (keg * Ke) / kee).LengthSquared();
if (distSq < radiusSq)
return true;
// a * t^2 + b * t + c = 0
float a = kee * kss - kes * kes;
if (System.Math.Abs(a) < JiggleMath.Epsilon)
return false;
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)
return false;
// solve for t - take minimum
float t = (-b - (float)System.Math.Sqrt((float)blah)) / (2.0f * a);
if (t < 0.0f || t > 1.0f)
return false;
tS = t;
return true;
}
/// <summary>
/// Intersect a segment with the world. If non-zero the predicate
/// allows certain skins to be excluded
/// </summary>
/// <param name="seg"></param>
/// <param name="collisionPredicate"></param>
/// <returns></returns>
public abstract bool SegmentIntersect(out float fracOut, out CollisionSkin skinOut, out Vector3 posOut, out Vector3 normalOut,
Segment seg, CollisionSkinPredicate1 collisionPredicate);
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)
{
Vector3 orientationBackward = MatrixHelper.GetBackward(transform.Orientation);
pos = seg.GetPoint(frac);
normal = pos - transform.Position;
normal -= Vector3.Dot(normal, orientationBackward) * orientationBackward;
JiggleMath.NormalizeSafe(ref normal);
}
else
{
pos = normal = Vector3.Zero;
}
return result;
}
/// <summary>
/// GetBoxTriangleIntersectionPoints
/// Pushes intersection points onto the back of pts. Returns the
/// number of points found.
/// Points that are close together (compared to
/// combinationDistance) get combined
/// </summary>
/// <param name="pts"></param>
/// <param name="box"></param>
/// <param name="triangle"></param>
/// <param name="combinationDistance"></param>
/// <returns></returns>
private static int GetBoxTriangleIntersectionPoints(List<Vector3> pts, Box box, Triangle triangle, float combinationDistance)
{
// first intersect each edge of the box with the triangle
Box.Edge[] edges;
box.GetEdges(out edges);
Vector3[] boxPts;
box.GetCornerPoints(out boxPts);
float tS;
float tv1, tv2;
int iEdge;
for (iEdge = 0; iEdge < 12; ++iEdge)
{
Box.Edge edge = edges[iEdge];
Segment seg = new Segment(boxPts[(int)edge.Ind0], boxPts[(int)edge.Ind1] - boxPts[(int)edge.Ind0]);
if (Intersection.SegmentTriangleIntersection(out tS, out tv1, out tv2, seg, triangle))
{
AddPoint(pts, seg.GetPoint(tS), combinationDistance * combinationDistance);
}
}
Vector3 pos, n;
// now each edge of the triangle with the box
for (iEdge = 0; iEdge < 3; ++iEdge)
{
Vector3 pt0 = triangle.GetPoint(iEdge);
Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3);
Segment s1 = new Segment(pt0, pt1 - pt0);
Segment s2 = new Segment(pt1, pt0 - pt1);
if (box.SegmentIntersect(out tS, out pos, out n, s1))
AddPoint(pts, pos, combinationDistance * combinationDistance);
if (box.SegmentIntersect(out tS, out pos, out n, s2))
AddPoint(pts, pos, combinationDistance * combinationDistance);
}
return pts.Count;
}
/// <summary>
///
/// </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 * MatrixHelper.GetBackward(oldCapsule.Orientation));
Segment newSeg = new Segment(newCapsule.Position, newCapsule.Length * MatrixHelper.GetBackward(newCapsule.Orientation));
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 (MathHelper.Min(oldDistSq, newDistSq) < ((radius + collTolerance) * (radius + collTolerance)))
{
Matrix oldBoxOrientation = oldBox.Orientation;
Vector3 segPos = oldSeg.GetPoint(oldSegT);
Vector3 boxPos = oldBox.GetCentre() + oldBoxT0 * MatrixHelper.GetRight(ref oldBoxOrientation) +
oldBoxT1 * MatrixHelper.GetUp(ref oldBoxOrientation) + oldBoxT2 * MatrixHelper.GetBackward(ref oldBoxOrientation);
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.TransformCoordinate(Vector3Helper.Backward, Matrix.RotationAxis(Vector3Helper.Up, MathHelper.ToRadians(random.Next(360))));
}
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(boxPos - body0Pos, boxPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref dir, &collInfo, 1);
}
}
}
/// <summary>
/// Must support intersection with a segment (ray cast)
/// </summary>
/// <param name="frac"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns></returns>
public abstract bool SegmentIntersect(out float frac,out Vector3 pos,
out Vector3 normal,Segment seg);
/// <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.TransformCoordinate(pos, carBody.Orientation);// *mPos;
Vector3 worldAxis = Vector3.TransformCoordinate(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.TransformCoordinate(MatrixHelper.GetRight(carBody.Orientation), 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.TransformCoordinate( 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.TransformCoordinate(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;
}
/// <summary>
/// Every skin must support a ray/segment intersection test -
/// operates on the new value of the primitives
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns></returns>
public bool SegmentIntersect(out float frac, out Vector3 pos, out Vector3 normal, Segment seg)
{
Vector3 segEnd = seg.GetEnd();
frac = float.MaxValue;
float thisSegLenRelToOrig = 1.0f;
Segment segCopy = seg;
pos = normal = Vector3.Zero;
for (int prim = primitivesNewWorld.Count; prim-- != 0; )
{
float thisFrac;
Vector3 newPosition = pos;
if (primitivesNewWorld[prim].SegmentIntersect(out thisFrac, out newPosition, out normal, segCopy))
{
pos = newPosition;
frac = thisFrac * thisSegLenRelToOrig;
segCopy.Delta *= thisFrac;
thisSegLenRelToOrig *= frac;
}
}
//System.Diagnostics.Debug.WriteLineIf(frac <= 1.0f, pos);
return (frac <= 1.0f);
}
public override bool SegmentIntersect(out float fracOut, out Vector3 posOut, out Vector3 normalOut, Segment seg)
{
fracOut = float.MaxValue;
posOut = normalOut = Vector3.Zero;
// algo taken from p674 of realting rendering
// needs debugging
float min = float.MinValue;
float max = float.MaxValue;
Vector3 p = GetCentre() - seg.Origin;
Vector3 h;
h.X = sideLengths.X * 0.5f;
h.Y = sideLengths.Y * 0.5f;
h.Z = sideLengths.Z * 0.5f;
int dirMax = 0;
int dirMin = 0;
int dir = 0;
Matrix orientation = transform.Orientation;
Vector3[] matrixVec = new Vector3[3];
MatrixHelper.GetRight(ref orientation, out matrixVec[0]); //matrixVec[0] = transform.Orientation.Right;
MatrixHelper.GetUp(ref orientation, out matrixVec[1]); //matrixVec[1] = transform.Orientation.Up;
MatrixHelper.GetBackward(ref orientation, out matrixVec[2]); //matrixVec[2] = transform.Orientation.Backward;
float[] vectorFloat = new float[3];
vectorFloat[0] = h.X;
vectorFloat[1] = h.Y;
vectorFloat[2] = h.Z;
for (dir = 0; dir < 3; dir++)
{
float e = Vector3.Dot(matrixVec[dir], p);
float f = Vector3.Dot(matrixVec[dir], seg.Delta);
if (System.Math.Abs(f) > JiggleMath.Epsilon)
{
float t1 = (e + vectorFloat[dir]) / f;
float t2 = (e - vectorFloat[dir]) / f;
if (t1 > t2){float tmp = t1;t1 = t2; t2 = tmp;}
if (t1 > min)
{
min = t1;
dirMin = dir;
}
if (t2 < max)
{
max = t2;
dirMax = dir;
}
if (min > max)
return false;
if (max < 0.0f)
return false;
}
else if((-e-vectorFloat[dir] > 0.0f) ||
(-e + vectorFloat[dir] < 0.0f))
{
return false;
}
}
if (min > 0.0f)
{
dir = dirMin;
fracOut = min;
}
else
{
dir = dirMax;
fracOut = max;
}
fracOut = MathHelper.Clamp(fracOut, 0.0f, 1.0f);
posOut = seg.GetPoint(fracOut);
if (Vector3.Dot(matrixVec[dir], seg.Delta) > 0.0f)
normalOut = -matrixVec[dir];
else
normalOut = matrixVec[dir];
return true;
}
public override bool SegmentIntersect(out float fracOut,out CollisionSkin skinOut,out Vector3 posOut,out Vector3 normalOut, Segment seg, CollisionSkinPredicate1 collisionPredicate)
{
int numSkins = skins.Count;
BoundingBox segBox = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddSegment(seg, ref segBox);
//initialise the outputs
fracOut = float.MaxValue;
skinOut = null;
posOut = normalOut = Vector3.Zero;
// working vars
float frac;
Vector3 pos;
Vector3 normal;
for (int iskin = 0; iskin < numSkins; ++iskin)
{
CollisionSkin skin = skins[iskin];
if ((collisionPredicate == null) ||
collisionPredicate.ConsiderSkin(skin))
{
// basic bbox test
if (BoundingBoxHelper.OverlapTest(ref skin.WorldBoundingBox, ref segBox))
{
if (skin.SegmentIntersect(out frac, out pos, out normal, seg))
{
if (frac < fracOut)
{
posOut = pos;
normalOut = normal;
skinOut = skin;
fracOut = frac;
}
}
}
}
}
if (fracOut > 1.0f) return false;
fracOut = MathHelper.Clamp(fracOut, 0.0f, 1.0f);
return true;
}