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
Capsule oldCapsule0 = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule0 = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
Segment oldSeg0 = new Segment(oldCapsule0.Position, oldCapsule0.Length * oldCapsule0.Orientation.Backward);
Segment newSeg0 = new Segment(newCapsule0.Position, newCapsule0.Length * newCapsule0.Orientation.Backward);
Capsule oldCapsule1 = (Capsule)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
Capsule newCapsule1 = (Capsule)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Segment oldSeg1 = new Segment(oldCapsule1.Position, oldCapsule1.Length * oldCapsule1.Orientation.Backward);
Segment newSeg1 = new Segment(newCapsule1.Position, newCapsule1.Length * newCapsule1.Orientation.Backward);
float radSum = newCapsule0.Radius + newCapsule1.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.Transform(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = pos1 +
(oldCapsule1.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>bool</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 override bool SegmentIntersect(out float fracOut, out CollisionSkin skinOut, out Microsoft.Xna.Framework.Vector3 posOut, out Microsoft.Xna.Framework.Vector3 normalOut, JigLibX.Geometry.Segment seg, CollisionSkinPredicate1 collisionPredicate)
{
fracOut = float.MaxValue;
skinOut = null;
posOut = normalOut = Vector3.Zero;
Vector3 min = seg.GetPoint(0);
Vector3 tmp = seg.GetEnd();
Vector3 max;
Vector3.Max(ref min, ref tmp, out max);
Vector3.Min(ref min, ref tmp, out min);
BoundingBox box = new BoundingBox(min, max);
float frac;
Vector3 pos;
Vector3 normal;
active_.Clear();
Extract(min, max, active_);
int nActive = active_.Count;
for (int i = 0; i != nActive; ++i)
{
CollisionSkin skin = active_[i];
if (collisionPredicate == null || collisionPredicate.ConsiderSkin(skin))
{
if (BoundingBoxHelper.OverlapTest(ref box, ref skin.WorldBoundingBox))
{
if (skin.SegmentIntersect(out frac, out pos, out normal, seg))
{
if (frac >= 0 && frac < fracOut)
{
fracOut = frac;
skinOut = skin;
posOut = pos;
normalOut = normal;
}
}
}
}
}
return(fracOut <= 1);
}
/// <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)
{
frac = 0;
pos = Vector3.Zero;
normal = Vector3.Up;
//if (seg.Delta.Y > -JiggleMath.Epsilon )
// return false;
Vector3 normalStart;
float heightStart;
GetHeightAndNormal(out heightStart, out normalStart,seg.Origin);
if (heightStart < 0.0f)
return false;
Vector3 normalEnd;
float heightEnd;
Vector3 end = seg.GetEnd();
GetHeightAndNormal(out heightEnd, out normalEnd,end);
if (heightEnd > 0.0f)
return false;
// start is above, end is below...
float depthEnd = -heightEnd;
// normal is the weighted mean of these...
float weightStart = 1.0f / (JiggleMath.Epsilon + heightStart);
float weightEnd = 1.0f / (JiggleMath.Epsilon + depthEnd);
normal = (normalStart * weightStart + normalEnd * weightEnd) /
(weightStart + weightEnd);
frac = heightStart / (heightStart + depthEnd + JiggleMath.Epsilon);
pos = seg.GetPoint(frac);
return true;
}
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;
Vector3[] matrixVec = new Vector3[3];
matrixVec[0] = transform.Orientation.Right;
matrixVec[1] = transform.Orientation.Up;
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>
/// 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 (MathHelper.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(Vector3.Backward, Matrix.CreateFromAxisAngle(Vector3.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>
/// 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)
{
// move segment into octree space
seg.Origin = Vector3.Transform(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.Transform(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 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, capsule.Orientation.Backward),
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, capsule.Orientation.Backward) < 0.0f)
sideFrac = float.MaxValue;
else if (Vector3.Dot(sidePos - capsule.GetEnd(), capsule.Orientation.Backward) > 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.Position, capsule.Radius));
bestFrac = MathHelper.Min(sideFrac, originFrac);
bestFrac = MathHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return true;
}
return false;
}
/// <summary>
/// SegmentIntersect
/// </summary>
/// <param name="fracOut"></param>
/// <param name="posOut"></param>
/// <param name="normalOut"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
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;
// BEN-OPTIMISATION: Faster code.
Vector3 centre = GetCentre();
Vector3 p;
Vector3.Subtract(ref centre, ref seg.Origin, out p);
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;
// BEN-OPTIMISATIOIN: Ugly inlining and variable reuse for marginal speed increase.
#region "Original Code"
/*
Vector3[] matrixVec = new Vector3[3];
matrixVec[0] = transform.Orientation.Right;
matrixVec[1] = transform.Orientation.Up;
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;
}
}
*/
#endregion
#region "Faster code albeit scarier code!"
float e = Vector3.Dot(transform.Orientation.Right(), p);
float f = Vector3.Dot(transform.Orientation.Right(), seg.Delta);
if (System.Math.Abs(f) > JiggleMath.Epsilon)
{
float t1 = (e + h.X) / f;
float t2 = (e - h.X) / f;
if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
if (t1 > min)
{
min = t1;
dirMin = 0;
}
if (t2 < max)
{
max = t2;
dirMax = 0;
}
if (min > max)
return false;
if (max < 0.0f)
return false;
}
else if ((-e - h.X > 0.0f) || (-e + h.X < 0.0f))
{
return false;
}
e = Vector3.Dot(transform.Orientation.Up(), p);
f = Vector3.Dot(transform.Orientation.Up(), seg.Delta);
if (System.Math.Abs(f) > JiggleMath.Epsilon)
{
float t1 = (e + h.Y) / f;
float t2 = (e - h.Y) / f;
if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
if (t1 > min)
{
min = t1;
dirMin = 1;
}
if (t2 < max)
{
max = t2;
dirMax = 1;
}
if (min > max)
return false;
if (max < 0.0f)
return false;
}
else if ((-e - h.Y > 0.0f) || (-e + h.Y < 0.0f))
{
return false;
}
e = Vector3.Dot(transform.Orientation.Backward(), p);
f = Vector3.Dot(transform.Orientation.Backward(), seg.Delta);
if (System.Math.Abs(f) > JiggleMath.Epsilon)
{
float t1 = (e + h.Z) / f;
float t2 = (e - h.Z) / f;
if (t1 > t2) { float tmp = t1; t1 = t2; t2 = tmp; }
if (t1 > min)
{
min = t1;
dirMin = 2;
}
if (t2 < max)
{
max = t2;
dirMax = 2;
}
if (min > max)
return false;
if (max < 0.0f)
return false;
}
else if ((-e - h.Z > 0.0f) || (-e + h.Z < 0.0f))
{
return false;
}
#endregion
if (min > 0.0f)
{
dir = dirMin;
fracOut = min;
}
else
{
dir = dirMax;
fracOut = max;
}
if (dir == 0)
{
fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f);
posOut = seg.GetPoint(fracOut);
if (Vector3.Dot(transform.Orientation.Right(), seg.Delta) > 0.0f)
normalOut = -transform.Orientation.Right();
else
normalOut = transform.Orientation.Right();
}
else if (dir == 1)
{
fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f);
posOut = seg.GetPoint(fracOut);
if (Vector3.Dot(transform.Orientation.Up(), seg.Delta) > 0.0f)
normalOut = -transform.Orientation.Up();
else
normalOut = transform.Orientation.Up();
}
else
{
fracOut = OpenTKHelper.Clamp(fracOut, 0.0f, 1.0f);
posOut = seg.GetPoint(fracOut);
if (Vector3.Dot(transform.Orientation.Backward(), seg.Delta) > 0.0f)
normalOut = -transform.Orientation.Backward();
else
normalOut = transform.Orientation.Backward();
}
return true;
}
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;
}
/// <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;
// get the skins in the order that we're expecting
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
Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
Capsule oldCapsule = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as Capsule;
Capsule newCapsule = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as Capsule;
Segment oldSeg = new Segment(oldCapsule.Position, oldCapsule.Length * oldCapsule.Orientation.Backward());
Segment newSeg = new Segment(oldCapsule.Position, newCapsule.Length * newCapsule.Orientation.Backward());
float radSum = newCapsule.Radius + newSphere.Radius;
float oldt, newt;
float oldDistSq = Distance.PointSegmentDistanceSq(out oldt, oldSphere.Position, oldSeg);
float newDistSq = Distance.PointSegmentDistanceSq(out newt, newSphere.Position, newSeg);
if (OpenTKHelper.Min(oldDistSq, newDistSq) < (radSum + collTolerance) * (radSum + collTolerance))
{
Vector3 segPos = oldSeg.GetPoint(oldt);
Vector3 delta = oldSphere.Position - segPos;
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 = Vector3Extensions.TransformNormal(Vector3Extensions.Backward, Matrix4.CreateFromAxisAngle(Vector3Extensions.Up, OpenTKHelper.ToRadians(random.Next(360))));
}
Vector3 worldPos = segPos +
((oldCapsule.Radius - 0.5f * depth) * delta);
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos,
worldPos - body1Pos, depth);
collisionFunctor.CollisionNotify(ref info, ref delta, &collInfo, 1);
}
}
}
/// <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;
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>
/// 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>
/// 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>int</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;
// BEN-OPTIMISATION: Allocating just one Vector3 to be reused.
Vector3 point = new Vector3();
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))
{
// BEN-OPTIMISATION: Reusing the existing point variable instead allocating new ones.
// This also allows point to be based by reference.
seg.GetPoint(ref point, tS);
AddPoint(pts, ref point, combinationDistance * combinationDistance);
}
}
Vector3 pos, n;
// now each edge of the triangle with the box
for (iEdge = 0; iEdge < 3; ++iEdge)
{
#region "BEN-OPTIMISATION: Remove excess allocations and pass variables by reference."
// ORIGINAL CODE:
/*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);*/
// OPTIMISED CODE:
Vector3 pt0 = triangle.GetPoint(iEdge);
Vector3 pt1 = triangle.GetPoint((iEdge + 1) % 3);
Vector3 difference1;
Vector3 difference2;
Vector3.Subtract(ref pt1, ref pt0, out difference1);
Vector3.Subtract(ref pt0, ref pt1, out difference2);
Segment s1 = new Segment(ref pt0, ref difference1);
Segment s2 = new Segment(ref pt1, ref difference2);
#endregion
if (box.SegmentIntersect(out tS, out pos, out n, s1))
AddPoint(pts, ref pos, combinationDistance * combinationDistance);
if (box.SegmentIntersect(out tS, out pos, out n, s2))
AddPoint(pts, ref pos, combinationDistance * combinationDistance);
}
return pts.Count;
}
/// <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();
Matrix4 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
Matrix4 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
}
}
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;
}
/// <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)
{
// move segment into octree space
seg.Origin = Vector3.Transform(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[DetectFunctor.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.Transform(seg.GetPoint(thisFrac), transformMatrix);
normal = Vector3.TransformNormal(meshTriangle.Plane.Normal, transformMatrix);
}
}
}
frac = bestFrac;
if (bestFrac < float.MaxValue)
{
#if !USE_STACKALLOC
DetectFunctor.FreeStackAlloc(potTriArray);
#endif
return(true);
}
else
{
#if !USE_STACKALLOC
DetectFunctor.FreeStackAlloc(potTriArray);
#endif
return(false);
}
}
}
}
