/// <summary>
/// Indicates if a segment intersects a triangle
/// </summary>
/// <param name="seg"></param>
/// <param name="triangle"></param>
/// <returns>bool</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 static float PointSegmentDistance(out float t, Vector3 pt, Segment seg)
{
Vector3 kDiff;
float fT;
Vector3.Subtract(ref pt, ref seg.Origin, out kDiff);
Vector3.Dot(ref kDiff, ref seg.Delta, out fT);
if (fT <= 0.0f)
{
fT = 0.0f;
}
else
{
float sqrLen = seg.Delta.LengthSquared();
if (fT >= sqrLen)
{
fT = 1.0f;
kDiff = kDiff - seg.Delta;
}
else
{
fT = fT / sqrLen;
kDiff = kDiff - (fT * seg.Delta);
}
}
t = fT;
return kDiff.Length();
}
public bool Intersects(
ref Vector3 position,
ref Vector3 vector,
out Vector3 resultPosition,
out Vector3 resultNormal,
out float resultFraction)
{
var segment = new Segment(position, vector);
return SegmentIntersect(out resultFraction, out resultPosition, out resultNormal, segment);
}
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 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;
}
public void OnFire(Vector3 muzzlePosition, Vector3 muzzleDir)
{
if (coolDownTimeRemaining <= 0)
{
//fpsModel.SetAnimationLayer("Pistol_Idle", 0.0f);
fpsModel.GetAnimationLayer().AddAnimation("Pistol_Fire", true);
//fpsModel.SetAnimationLayer("Pistol_Fire", 1.0f);
coolDownTimeRemaining = ResourceManager.Inst.GetAnimation("Pistol_Fire").EndTime;
float dist;
CollisionSkin skin;
Vector3 pos, normal;
Segment seg = new Segment(muzzlePosition, muzzleDir * 50);
scene.GetPhysicsEngine().CollisionSystem.SegmentIntersect(out dist, out skin, out pos, out normal, seg, ignorePred);
if (skin != null)
{
Decal decal = new Decal();
decal.SetMaterial("BulletMetal1");
decal.Normal = normal;
decal.Scale = new Vector2(0.25f, 0.25f);
Vector3 posNew = seg.Origin + seg.Delta * dist;
decal.Transformation.SetPosition(posNew);
decal.IsPersistent = false;
scene.AddEntity("bullet", decal);
ParticleEffect bulletEffect = ResourceManager.Inst.GetParticleEffect("BulletEffect");
ParticleEmitter collideEmitter = new ParticleEmitter(bulletEffect, 16);
collideEmitter.EmitOnce = true;
NormalTransform newTransform = new NormalTransform();
newTransform.ConformToNormal(normal);
newTransform.SetPosition(pos);
collideEmitter.Transformation = newTransform;
scene.AddEntity("bulletEmitter", collideEmitter);
}
}
}
InteractNode GetInteractNode(Microsoft.Xna.Framework.Ray lookRay)
{
float collisionDist;
CollisionSkin skin = null;
Vector3 pos, normal;
Segment seg = new Segment(lookRay.Position, lookRay.Direction * MAX_INTERACT_DIST);
scene.GetPhysicsEngine().CollisionSystem.SegmentIntersect(out collisionDist, out skin, out pos, out normal, seg, pred);
float bestDist = (skin != null) ? collisionDist : float.PositiveInfinity;
InteractNode bestNode = null;
for (int i = 0; i < interactables.Count; i++)
{
InteractObject interactObj = interactables[i].GetInteractObject();
BoundingBox bounds = interactObj.Transformation.TransformBounds(interactObj.GetMesh().GetBounds());
float? intersection = lookRay.Intersects(bounds);
if (intersection.HasValue)
{
float dist = intersection.Value;
if (dist < bestDist && dist <= MAX_INTERACT_DIST)
{
InteractNode node = interactObj.GetInteractNode();
if (node.IsEnabled())
{
bestDist = dist;
bestNode = interactObj.GetInteractNode();
}
}
}
}
return bestNode;
}
/// <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;
}
/// <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>
/// SegmentIntersect
/// </summary>
/// <param name="fracOut"></param>
/// <param name="skinOut"></param>
/// <param name="posOut"></param>
/// <param name="normalOut"></param>
/// <param name="seg"></param>
/// <param name="collisionPredicate"></param>
/// <returns>bool</returns>
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;
float frac;
Vector3 pos;
Vector3 normal;
Vector3 segmentBeginning = seg.Origin;
Vector3 segmentEnd = seg.Origin + seg.Delta;
Vector3 min = Vector3.Min(segmentBeginning, segmentEnd);
Vector3 max = Vector3.Max(segmentBeginning, segmentEnd);
active_.Clear();
BoundingBox box = new BoundingBox(min, max);
Extract(min, max, active_);
float distanceSquared = float.MaxValue;
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)
{
float newDistanceSquared = Vector3.DistanceSquared(segmentBeginning, pos);
if (newDistanceSquared < distanceSquared)
{
distanceSquared = newDistanceSquared;
fracOut = frac;
skinOut = skin;
posOut = pos;
normalOut = normal;
}
}
}
}
}
}
return(fracOut <= 1);
}
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>
/// SegmentBoxDistanceSq
/// </summary>
/// <param name="pfLParam"></param>
/// <param name="pfBParam0"></param>
/// <param name="pfBParam1"></param>
/// <param name="pfBParam2"></param>
/// <param name="rkSeg"></param>
/// <param name="rkBox"></param>
/// <returns>float</returns>
public static float SegmentBoxDistanceSq(out float pfLParam,
out float pfBParam0, out float pfBParam1, out float pfBParam2,
Segment rkSeg, Box rkBox)
{
pfLParam = pfBParam0 = pfBParam1 = pfBParam2 = 0.0f;
Line line = new Line(rkSeg.Origin, rkSeg.Delta);
float fLP, fBP0, fBP1, fBP2;
float fSqrDistance = SqrDistance(line, rkBox, out fLP, out fBP0, out fBP1, out fBP2);
if (fLP >= 0.0f)
{
if (fLP <= 1.0f)
{
pfLParam = fLP;
pfBParam0 = fBP0;// + 0.5f;
pfBParam1 = fBP1;// + 0.5f;
pfBParam2 = fBP2;// + 0.5f;
return MathHelper.Max(fSqrDistance, 0.0f);
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin + rkSeg.Delta,
rkBox, out pfBParam0, out pfBParam1, out pfBParam2);
pfLParam = 1.0f;
return MathHelper.Max(fSqrDistance, 0.0f);
}
}
else
{
fSqrDistance = SqrDistance(rkSeg.Origin, rkBox, out pfBParam0, out pfBParam1, out pfBParam2);
pfLParam = 0.0f;
return MathHelper.Max(fSqrDistance, 0.0f);
}
}
/// <summary>
/// SegmentTriangleDistanceSq
/// </summary>
/// <param name="segT"></param>
/// <param name="triT0"></param>
/// <param name="triT1"></param>
/// <param name="seg"></param>
/// <param name="triangle"></param>
/// <returns>float</returns>
public static float SegmentTriangleDistanceSq(out float segT, out float triT0, out float triT1, Segment seg, Triangle triangle)
{
// compare segment to all three edges of the triangle
float distSq = float.MaxValue;
if (Intersection.SegmentTriangleIntersection(out segT, out triT0, out triT1, seg, triangle))
{
segT = triT0 = triT1 = 0.0f;
return 0.0f;
}
float s, t, u;
float distEdgeSq;
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge0));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = t;
triT1 = 0.0f;
}
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin, triangle.Edge1));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = 0.0f;
triT1 = t;
}
distEdgeSq = SegmentSegmentDistanceSq(out s, out t, seg, new Segment(triangle.Origin + triangle.Edge0, triangle.Edge2));
if (distEdgeSq < distSq)
{
distSq = distEdgeSq;
segT = s;
triT0 = 1.0f - t;
triT1 = t;
}
// compare segment end points to triangle interior
float startTriSq = PointTriangleDistanceSq(out t, out u, seg.Origin, triangle);
if (startTriSq < distSq)
{
distSq = startTriSq;
segT = 0.0f;
triT0 = t;
triT1 = u;
}
float endTriSq = PointTriangleDistanceSq(out t, out u, seg.GetEnd(), triangle);
if (endTriSq < distSq)
{
distSq = endTriSq;
segT = 1.0f;
triT0 = t;
triT1 = u;
}
return distSq;
}
public void OnUpdate()
{
fpsModel.OnUpdate();
if (coolDownTimeRemaining > 0)
{
coolDownTimeRemaining -= Time.GameTime.ElapsedTime;
}
if (timeTilFire > 0)
timeTilFire -= Time.GameTime.ElapsedTime;
if (timeTilFire <= 0 && !hasFired)
{
hasFired = true;
if(!datablock.IsMelee)
new Sound3D(datablock.GetSoundEffect((ammo > 0)?WeaponAnimations.Fire:WeaponAnimations.Empty), muzzlePos);
if (ammo > 0 || datablock.IsMelee)
{
ammo--;
float dist;
CollisionSkin skin;
Vector3 pos, normal;
Segment seg = new Segment(muzzlePos, muzzleDir * datablock.FireDistance);
Microsoft.Xna.Framework.Ray shootRay = new Microsoft.Xna.Framework.Ray(muzzlePos, muzzleDir);
bool anyActorHit = false;
scene.GetPhysicsEngine().CollisionSystem.SegmentIntersect(out dist, out skin, out pos, out normal, seg, ignorePred);
float segDist = (skin != null) ? Vector3.Distance(seg.Delta * dist, Vector3.Zero): float.PositiveInfinity;
for (int i = 0; i < scene.Actors.Count; i++)
{
Actor currActor = scene.Actors[i];
if (currActor != scene.MainPlayer)
{
float shootDist;
HitType hit = currActor.GetHit(shootRay, datablock.FireDistance, out shootDist);
if (hit != HitType.None)
{
if (skin == null || (shootDist <= segDist))
{
currActor.ApplyDamage(datablock.Damage);
ParticleEffect bloodEffect = ResourceManager.Inst.GetParticleEffect("BloodEffect");
ParticleEmitter collideEmitter = new ParticleEmitter(bloodEffect, 16);
collideEmitter.EmitOnce = true;
NormalTransform newTransform = new NormalTransform();
newTransform.ConformToNormal(-muzzleDir);
newTransform.SetPosition(muzzlePos + muzzleDir*shootDist);
collideEmitter.Transformation = newTransform;
scene.AddEntity("bloodEmitter", collideEmitter);
anyActorHit = true;
}
}
}
}
if (skin != null)
{
ParticleEffect bulletEffect = ResourceManager.Inst.GetParticleEffect("BulletEffect");
ParticleEmitter collideEmitter = new ParticleEmitter(bulletEffect, 16);
collideEmitter.EmitOnce = true;
NormalTransform newTransform = new NormalTransform();
newTransform.ConformToNormal(normal);
newTransform.SetPosition(pos);
collideEmitter.Transformation = newTransform;
scene.AddEntity("bulletEmitter", collideEmitter);
}
if(datablock.IsMelee)
new Sound3D(datablock.GetSoundEffect((anyActorHit) ? WeaponAnimations.SuccessHit : WeaponAnimations.Fire), muzzlePos);
}
}
}
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;
}
/// <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
}
}
/// <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;
}
/// <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 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>
/// 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);
/// <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>
/// PointSegmentDistanceSq
/// </summary>
/// <param name="pt"></param>
/// <param name="seg"></param>
/// <param name="result"></param>
public static void PointSegmentDistanceSq(ref Vector3 pt, ref Segment seg, out float result)
{
Vector3 kDiff;
float fT;
Vector3.Subtract(ref pt, ref seg.Origin, out kDiff);
Vector3.Dot(ref kDiff, ref seg.Delta, out fT);
if (fT <= 0.0f)
{
fT = 0.0f;
}
else
{
float sqrLen = seg.Delta.LengthSquared();
if (fT >= sqrLen)
{
fT = 1.0f;
kDiff = kDiff - seg.Delta;
}
else
{
fT = fT / sqrLen;
kDiff = kDiff - (fT * seg.Delta);
}
}
result = kDiff.LengthSquared();
}
/// <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>bool</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);
}
/// <summary>
/// Returns the distance of two segments.
/// </summary>
/// <param name="t0">Parametric representation of nearest point on seg0.</param>
/// <param name="t1">Parametric representation of nearest point on seg0.</param>
/// <param name="seg0">First segment to test.</param>
/// <param name="seg1">Second segment to test.</param>
/// <returns>float</returns>
public static float SegmentSegmentDistanceSq(out float t0, out float t1, Segment seg0, Segment seg1)
{
Vector3 kDiff = seg0.Origin - seg1.Origin;
float fA00 = seg0.Delta.LengthSquared();
float fA01 = -Vector3.Dot(seg0.Delta, seg1.Delta);
float fA11 = seg1.Delta.LengthSquared();
float fB0 = Vector3.Dot(kDiff, seg0.Delta);
float fC = kDiff.LengthSquared();
float fDet = System.Math.Abs(fA00 * fA11 - fA01 * fA01);
float fB1, fS, fT, fSqrDist, fTmp;
if (fDet >= JiggleMath.Epsilon)
{
// line segments are not parallel
fB1 = -Vector3.Dot(kDiff, seg1.Delta);
fS = fA01 * fB1 - fA11 * fB0;
fT = fA01 * fB0 - fA00 * fB1;
if (fS >= 0.0f)
{
if (fS <= fDet)
{
if (fT >= 0.0f)
{
if (fT <= fDet) // region 0 (interior)
{
// minimum at two interior points of 3D lines
float fInvDet = 1.0f / fDet;
fS *= fInvDet;
fT *= fInvDet;
fSqrDist = fS * (fA00 * fS + fA01 * fT + 2.0f * fB0) +
fT * (fA01 * fS + fA11 * fT + 2.0f * fB1) + fC;
}
else // region 3 (side)
{
fT = 1.0f;
fTmp = fA01 + fB0;
if (fTmp >= 0.0f)
{
fS = 0.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else if (-fTmp >= fA00)
{
fS = 1.0f;
fSqrDist = fA00 + fA11 + fC + (2.0f) * (fB1 + fTmp);
}
else
{
fS = -fTmp / fA00;
fSqrDist = fTmp * fS + fA11 + (2.0f) * fB1 + fC;
}
}
}
else // region 7 (side)
{
fT = 0.0f;
if (fB0 >= 0.0f)
{
fS = 0.0f;
fSqrDist = fC;
}
else if (-fB0 >= fA00)
{
fS = 1.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else
{
fS = -fB0 / fA00;
fSqrDist = fB0 * fS + fC;
}
}
}
else
{
if (fT >= 0.0f)
{
if (fT <= fDet) // region 1 (side)
{
fS = 1.0f;
fTmp = fA01 + fB1;
if (fTmp >= 0.0f)
{
fT = 0.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else if (-fTmp >= fA11)
{
fT = 1.0f;
fSqrDist = fA00 + fA11 + fC + (2.0f) * (fB0 + fTmp);
}
else
{
fT = -fTmp / fA11;
fSqrDist = fTmp * fT + fA00 + (2.0f) * fB0 + fC;
}
}
else // region 2 (corner)
{
fTmp = fA01 + fB0;
if (-fTmp <= fA00)
{
fT = 1.0f;
if (fTmp >= 0.0f)
{
fS = 0.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else
{
fS = -fTmp / fA00;
fSqrDist = fTmp * fS + fA11 + (2.0f) * fB1 + fC;
}
}
else
{
fS = 1.0f;
fTmp = fA01 + fB1;
if (fTmp >= 0.0f)
{
fT = 0.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else if (-fTmp >= fA11)
{
fT = 1.0f;
fSqrDist = fA00 + fA11 + fC +
(2.0f) * (fB0 + fTmp);
}
else
{
fT = -fTmp / fA11;
fSqrDist = fTmp * fT + fA00 + (2.0f) * fB0 + fC;
}
}
}
}
else // region 8 (corner)
{
if (-fB0 < fA00)
{
fT = 0.0f;
if (fB0 >= 0.0f)
{
fS = 0.0f;
fSqrDist = fC;
}
else
{
fS = -fB0 / fA00;
fSqrDist = fB0 * fS + fC;
}
}
else
{
fS = 1.0f;
fTmp = fA01 + fB1;
if (fTmp >= 0.0f)
{
fT = 0.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else if (-fTmp >= fA11)
{
fT = 1.0f;
fSqrDist = fA00 + fA11 + fC + (2.0f) * (fB0 + fTmp);
}
else
{
fT = -fTmp / fA11;
fSqrDist = fTmp * fT + fA00 + (2.0f) * fB0 + fC;
}
}
}
}
}
else
{
if (fT >= 0.0f)
{
if (fT <= fDet) // region 5 (side)
{
fS = 0.0f;
if (fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else if (-fB1 >= fA11)
{
fT = 1.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else
{
fT = -fB1 / fA11;
fSqrDist = fB1 * fT + fC;
}
}
else // region 4 (corner)
{
fTmp = fA01 + fB0;
if (fTmp < 0.0f)
{
fT = 1.0f;
if (-fTmp >= fA00)
{
fS = 1.0f;
fSqrDist = fA00 + fA11 + fC + (2.0f) * (fB1 + fTmp);
}
else
{
fS = -fTmp / fA00;
fSqrDist = fTmp * fS + fA11 + (2.0f) * fB1 + fC;
}
}
else
{
fS = 0.0f;
if (fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else if (-fB1 >= fA11)
{
fT = 1.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else
{
fT = -fB1 / fA11;
fSqrDist = fB1 * fT + fC;
}
}
}
}
else // region 6 (corner)
{
if (fB0 < 0.0f)
{
fT = 0.0f;
if (-fB0 >= fA00)
{
fS = 1.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else
{
fS = -fB0 / fA00;
fSqrDist = fB0 * fS + fC;
}
}
else
{
fS = 0.0f;
if (fB1 >= 0.0f)
{
fT = 0.0f;
fSqrDist = fC;
}
else if (-fB1 >= fA11)
{
fT = 1.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else
{
fT = -fB1 / fA11;
fSqrDist = fB1 * fT + fC;
}
}
}
}
}
else
{
// line segments are parallel
if (fA01 > 0.0f)
{
// direction vectors form an obtuse angle
if (fB0 >= 0.0f)
{
fS = 0.0f;
fT = 0.0f;
fSqrDist = fC;
}
else if (-fB0 <= fA00)
{
fS = -fB0 / fA00;
fT = 0.0f;
fSqrDist = fB0 * fS + fC;
}
else
{
fB1 = -Vector3.Dot(kDiff, seg1.Delta);
fS = 1.0f;
fTmp = fA00 + fB0;
if (-fTmp >= fA01)
{
fT = 1.0f;
fSqrDist = fA00 + fA11 + fC + (2.0f) * (fA01 + fB0 + fB1);
}
else
{
fT = -fTmp / fA01;
fSqrDist = fA00 + (2.0f) * fB0 + fC + fT * (fA11 * fT +
(2.0f) * (fA01 + fB1));
}
}
}
else
{
// direction vectors form an acute angle
if (-fB0 >= fA00)
{
fS = 1.0f;
fT = 0.0f;
fSqrDist = fA00 + (2.0f) * fB0 + fC;
}
else if (fB0 <= 0.0f)
{
fS = -fB0 / fA00;
fT = 0.0f;
fSqrDist = fB0 * fS + fC;
}
else
{
fB1 = -Vector3.Dot(kDiff, seg1.Delta);
fS = 0.0f;
if (fB0 >= -fA01)
{
fT = 1.0f;
fSqrDist = fA11 + (2.0f) * fB1 + fC;
}
else
{
fT = -fB0 / fA01;
fSqrDist = fC + fT * ((2.0f) * fB1 + fA11 * fT);
}
}
}
}
t0 = fS;
t1 = fT;
return System.Math.Abs(fSqrDist);
}
/// <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>
/// Must support intersection with a segment (ray cast)
/// </summary>
/// <param name="frac"></param>
/// <param name="pos"></param>
/// <param name="normal"></param>
/// <param name="seg"></param>
/// <returns>bool</returns>
public abstract bool SegmentIntersect(out float frac,out Vector3 pos,
out Vector3 normal,Segment seg);
/// <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>
/// 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 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);
}
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>
/// 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);
}
}
}
