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