/// <summary>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(JVector impulse, JVector relativePosition)
{
if (this.isStatic)
{
throw new InvalidOperationException("Can't apply an impulse to a static body.");
}
JVector temp;
JVector.Multiply(ref impulse, inverseMass, out temp);
JVector.Add(ref linearVelocity, ref temp, out linearVelocity);
JVector.Cross(ref relativePosition, ref impulse, out temp);
JVector.Transform(ref temp, ref invInertiaWorld, out temp);
JVector.Add(ref angularVelocity, ref temp, out angularVelocity);
}
public override void PrepareForIteration(float timestep)
{
JVector.Transform(localAnchor1, body1.orientation, out r1);
JVector.Transform(localAnchor2, body2.orientation, out r2);
JVector.Add(body1.position, r1, out var p1);
JVector.Add(body2.position, r2, out var p2);
JVector.Subtract(p2, p1, out var dp);
float deltaLength = dp.Length();
var n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n = JVector.Normalize(n);
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = r2 % n;
effectiveMass = body1.inverseMass + body2.inverseMass
+ (JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1])
+ (JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]);
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
JVector temp;
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isMassPoint))
{
//exponential map
JVector axis;
float angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
JVector.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
JVector.Multiply(ref body.angularVelocity, ((float)Math.Sin(0.5f * angle * timestep) / angle), out axis);
}
JQuaternion dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timestep * 0.5f));
JQuaternion ornA; JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
{
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
}
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
{
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
}
body.Update();
}
public override void MakeHull(ref List <JVector> triangleList, int generationThreshold)
{
List <JVector> triangles = new List <JVector>();
for (int i = 0; i < shapes.Length; i++)
{
shapes[i].Shape.MakeHull(ref triangles, 4);
for (int e = 0; e < triangles.Count; e++)
{
JVector pos = triangles[e];
JVector.Transform(ref pos, ref shapes[i].orientation, out pos);
JVector.Add(ref pos, ref shapes[i].position, out pos);
triangleList.Add(pos);
}
triangles.Clear();
}
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
float jv = JVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += JVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
float softnessScalar = accumulatedImpulse * softnessOverDt;
float lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
float previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = JMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
float previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = JMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
JVector temp;
if (!body1.isStatic)
{
JVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
JVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
if (!body2.isStatic)
{
JVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
JVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref JBBox aabb, EChild child, out JBBox result)
{
JVector dims;
JVector.Subtract(ref aabb.Max, ref aabb.Min, out dims);
JVector.Multiply(ref dims, 0.5f, out dims);
JVector offset = JVector.Zero;
switch (child)
{
case EChild.PPP: offset = new JVector(1, 1, 1); break;
case EChild.PPM: offset = new JVector(1, 1, 0); break;
case EChild.PMP: offset = new JVector(1, 0, 1); break;
case EChild.PMM: offset = new JVector(1, 0, 0); break;
case EChild.MPP: offset = new JVector(0, 1, 1); break;
case EChild.MPM: offset = new JVector(0, 1, 0); break;
case EChild.MMP: offset = new JVector(0, 0, 1); break;
case EChild.MMM: offset = new JVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new JBBox();
result.Min = new JVector(offset.X * dims.X, offset.Y * dims.Y, offset.Z * dims.Z);
JVector.Add(ref result.Min, ref aabb.Min, out result.Min);
JVector.Add(ref result.Min, ref dims, out result.Max);
// expand it just a tiny bit just to be safe!
double extra = 0.00001f;
JVector temp; JVector.Multiply(ref dims, extra, out temp);
JVector.Subtract(ref result.Min, ref temp, out result.Min);
JVector.Add(ref result.Max, ref temp, out result.Max);
}
private void IntegrateCallback(object obj)
{
var body = obj as RigidBody;
JVector temp;
JVector.Multiply(ref body.linearVelocity, timestep, out temp);
JVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
JVector axis;
float angle = body.angularVelocity.Length();
if (angle < 0.001f)
{
JVector.Multiply(ref body.angularVelocity,
(0.5f*timestep - (timestep*timestep*timestep)*(0.020833333333f)*angle*angle),
out axis);
}
else
{
JVector.Multiply(ref body.angularVelocity, ((float) Math.Sin(0.5f*angle*timestep)/angle), out axis);
}
var dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (float) Math.Cos(angle*timestep*0.5f));
JQuaternion ornA;
JQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
JQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
JMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
JVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
body.SweptExpandBoundingBox(timestep);
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0);
vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1);
vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2);
JVector sum = vecs[0];
JVector.Add(ref sum, ref vecs[1], out sum);
JVector.Add(ref sum, ref vecs[2], out sum);
JVector.Multiply(ref sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(ref vecs[1], ref vecs[0], out sum);
JVector.Subtract(ref vecs[2], ref vecs[0], out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector p1, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
JVector l = lineNormal;
JVector t = (p1 - anchor) % l;
if (t.LengthSquared() != 0.0f)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t;
jacobian[1] = r1 % t;
effectiveMass = body1.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p1 - anchor)).Length() * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property.
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
///
/// </summary>
/// <param name="timeStep"></param>
public override void PreStep(float timeStep)
{
float damping = (float)Math.Pow(Damping, timeStep);
foreach (RigidBody body in bodies)
{
if (FluidBox.Contains(body.BoundingBox) != JBBox.ContainmentType.Disjoint)
{
JVector[] positions = samples[body.Shape];
float frac = 0.0f;
JVector currentCoord = JVector.Zero;
for (int i = 0; i < positions.Length; i++)
{
currentCoord = JVector.Transform(positions[i], body.Orientation);
currentCoord = JVector.Add(currentCoord, body.Position);
bool containsCoord = false;
if (fluidArea == null)
{
containsCoord = FluidBox.Contains(ref currentCoord) != JBBox.ContainmentType.Disjoint;
}
else
{
containsCoord = fluidArea(ref currentCoord);
}
if (containsCoord)
{
body.AddForce((1.0f / positions.Length) * body.Mass * Flow);
body.AddForce(-(1.0f / positions.Length) * body.Shape.Mass * Density * world.Gravity, currentCoord);
frac += 1.0f / positions.Length;
}
}
body.AngularVelocity *= damping;
body.LinearVelocity *= damping;
}
}
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref JVector point, ref JVector normal,
out JVector point1, out JVector point2)
{
JVector mn; JVector.Negate(ref normal, out mn);
JVector sA; SupportMapping(body1, shape1, ref mn, out sA);
JVector sB; SupportMapping(body2, shape2, ref normal, out sB);
JVector.Subtract(ref sA, ref point, out sA);
JVector.Subtract(ref sB, ref point, out sB);
float dot1 = JVector.Dot(ref sA, ref normal);
float dot2 = JVector.Dot(ref sB, ref normal);
JVector.Multiply(ref normal, dot1, out sA);
JVector.Multiply(ref normal, dot2, out sB);
JVector.Add(ref point, ref sA, out point1);
JVector.Add(ref point, ref sB, out point2);
}
/// <summary>
/// Add rigid-body to the draw-list to draw on the screen.
/// Only rigid-bodies with "Tag" = "DrawMe" are drawn.
/// </summary>
/// <param name="rb">Rigidbody</param>
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && (BodyTag)rb.Tag == BodyTag.DontDrawMe)
{
return;
}
Collider c = rb as Collider;
if (c?.GameObject.tag == ObjectTag.Ground)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (DisableRotation)
{
orientation = 0;
}
// particles don't rotate
if (isParticle)
{
this.inertia = 0.0f;
this.invInertia = 0.0f;
this.invOrientation = this.orientation = 0.0f;
this.boundingBox = shape.boundingBox;
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
angularVelocity = 0.0f;
}
else
{
// Given: Orientation, Inertia
// 3d version
//JMatrix.Transpose(ref orientation, out invOrientation);
invOrientation = -orientation;
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
JVector.Add(ref boundingBox.Min, ref this.position, out boundingBox.Min);
JVector.Add(ref boundingBox.Max, ref this.position, out boundingBox.Max);
if (!isStatic)
{
// 3d version
//JMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
//JMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
// we don't need a world inverse inertia
}
}
}
/// <summary>
/// The points in wolrd space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (!body1IsMassPoint)
{
JVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
}
JVector.Add(ref p1, ref body1.position, out p1);
if (!body2IsMassPoint)
{
JVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
}
JVector.Add(ref p2, ref body2.position, out p2);
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}
public void DebugDraw(IDebugDrawer drawer)
{
JVector pos1, pos2, pos3;
for (int i = 0; i < hullPoints.Count; i += 3)
{
pos1 = hullPoints[i + 0];
pos2 = hullPoints[i + 1];
pos3 = hullPoints[i + 2];
JVector.Transform(ref pos1, ref orientation, out pos1);
JVector.Add(ref pos1, ref position, out pos1);
JVector.Transform(ref pos2, ref orientation, out pos2);
JVector.Add(ref pos2, ref position, out pos2);
JVector.Transform(ref pos3, ref orientation, out pos3);
JVector.Add(ref pos3, ref position, out pos3);
drawer.DrawTriangle(pos1, pos2, pos3);
}
}
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.Default;
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.None;
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
// each quad has two triangles, called 'leftTriangle' and !'leftTriangle'
if (leftTriangle)
{
points[0].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1].Set((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2].Set((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
JVector sum = points[0];
JVector.Add(ref sum, ref points[1], out sum);
JVector.Add(ref sum, ref points[2], out sum);
JVector.Multiply(ref sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(ref points[1], ref points[0], out sum);
JVector.Subtract(ref points[2], ref points[0], out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
public override void SetCurrentShape(int index)
{
bool leftTriangle = false;
if (index >= numX * numZ)
{
leftTriangle = true;
index -= numX * numZ;
}
int quadIndexX = index % numX;
int quadIndexZ = index / numX;
if (leftTriangle)
{
points[0] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
else
{
points[0] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 0], (minZ + quadIndexZ + 0) * scaleZ);
points[1] = new JVector((minX + quadIndexX + 1) * scaleX, heights[minX + quadIndexX + 1, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
points[2] = new JVector((minX + quadIndexX + 0) * scaleX, heights[minX + quadIndexX + 0, minZ + quadIndexZ + 1], (minZ + quadIndexZ + 1) * scaleZ);
}
var sum = points[0];
JVector.Add(sum, points[1], out sum);
JVector.Add(sum, points[2], out sum);
JVector.Multiply(sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(points[1], points[0], out sum);
JVector.Subtract(points[2], points[0], out normal);
JVector.Cross(sum, normal, out normal);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector p1, dp;
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
float deltaLength = dp.Length();
JVector n = anchor - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
effectiveMass = body1.inverseMass + JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property (plus the condition below).
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector point, out JVector normal, out float penetration)
{
// Used variables
JVector temp1, temp2;
JVector v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector v41, v42, v4;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
JVector.Transform(ref v01, ref orientation1, out v01);
JVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
JVector.Transform(ref v02, ref orientation2, out v02);
JVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
JVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new JVector(0.00001f, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
JVector.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
JVector.Subtract(ref v12, ref v11, out v1);
if (JVector.Dot(ref v1, ref normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
JVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
JVector.Subtract(ref v1, ref v0, out normal);
normal.Normalize();
point = v11;
JVector.Add(ref point, ref v12, out point);
JVector.Multiply(ref point, 0.5f, out point);
JVector.Subtract(ref v12, ref v11, out temp1);
penetration = JVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
JVector.Subtract(ref v22, ref v21, out v2);
if (JVector.Dot(ref v2, ref normal) <= 0.0f)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
float dist = JVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0.0f)
{
JVector.Swap(ref v1, ref v2);
JVector.Swap(ref v11, ref v21);
JVector.Swap(ref v12, ref v22);
JVector.Negate(ref normal, out normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
JVector.Subtract(ref v32, ref v31, out v3);
if (JVector.Dot(ref v3, ref normal) <= 0.0f)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
JVector.Cross(ref v1, ref v3, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v2 = v3;
v21 = v31;
v22 = v32;
JVector.Subtract(ref v1, ref v0, out temp1);
JVector.Subtract(ref v3, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
JVector.Cross(ref v3, ref v2, out temp1);
if (JVector.Dot(ref temp1, ref v0) < 0.0f)
{
v1 = v3;
v11 = v31;
v12 = v32;
JVector.Subtract(ref v3, ref v0, out temp1);
JVector.Subtract(ref v2, ref v0, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
// Compute normal of the wedge face
JVector.Subtract(ref v2, ref v1, out temp1);
JVector.Subtract(ref v3, ref v1, out temp2);
JVector.Cross(ref temp1, ref temp2, out normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
// Compute distance from origin to wedge face
float d = JVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
JVector.Subtract(ref v42, ref v41, out v4);
JVector.Subtract(ref v4, ref v3, out temp1);
float delta = JVector.Dot(ref temp1, ref normal);
penetration = JVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
{
if (hit)
{
JVector.Cross(ref v1, ref v2, out temp1);
float b0 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v3, ref v2, out temp1);
float b1 = JVector.Dot(ref temp1, ref v0);
JVector.Cross(ref v0, ref v1, out temp1);
float b2 = JVector.Dot(ref temp1, ref v3);
JVector.Cross(ref v2, ref v1, out temp1);
float b3 = JVector.Dot(ref temp1, ref v0);
float sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
JVector.Cross(ref v2, ref v3, out temp1);
b1 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v3, ref v1, out temp1);
b2 = JVector.Dot(ref temp1, ref normal);
JVector.Cross(ref v1, ref v2, out temp1);
b3 = JVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
float inv = 1.0f / sum;
JVector.Multiply(ref v01, b0, out point);
JVector.Multiply(ref v11, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v21, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v31, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v02, b0, out temp2);
JVector.Add(ref temp2, ref point, out point);
JVector.Multiply(ref v12, b1, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v22, b2, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref v32, b3, out temp1);
JVector.Add(ref point, ref temp1, out point);
JVector.Multiply(ref point, inv * 0.5f, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
////// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//float d1 = JVector.Dot(ref temp1, ref v0);
////// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//float d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
JVector.Cross(ref v4, ref v0, out temp1);
float dot = JVector.Dot(ref temp1, ref v1);
if (dot >= 0.0f)
{
dot = JVector.Dot(ref temp1, ref v2);
if (dot >= 0.0f)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
dot = JVector.Dot(ref temp1, ref v3);
if (dot >= 0.0f)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
static void Main(string[] args)
{
var checker = new CollisionChecker();
var boxes1 = new List <TransformedShape>()
{
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 0, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 0, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 1, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 1, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 0, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 0, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 1, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 1, 1)),
};
var boxes2 = new List <TransformedShape>()
{
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 0, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 0, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 1, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(0, 1, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 0, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 0, 1)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 1, 0)),
new TransformedShape(new BoxShape(1, 1, 1), JMatrix.Identity, new JVector(1, 1, 1)),
};
var compoundShape1 = new CompoundShape(boxes1);
var rigidBody1 = new RigidBody(compoundShape1);
var compoundShape2 = new CompoundShape(boxes2);
var rigidBody2 = new RigidBody(compoundShape2);
rigidBody1.Position = new JVector(0, 0, 0);
rigidBody2.Position = new JVector(20, 0, 0);
var stepSize = new JVector(0.1f, 0, 0);
var stopWatch = new Stopwatch();
int stepCount = 0;
while (rigidBody1.Position.X < 20)
{
rigidBody1.Position = JVector.Add(rigidBody1.Position, stepSize);
stopWatch.Start();
bool colliding = checker.CheckCollision(rigidBody1, rigidBody2);
stopWatch.Stop();
stepCount++;
float pos1 = rigidBody1.Position.X;
float pos2 = rigidBody1.Position.X;
Console.WriteLine($"Position1 {pos1}; Position2 {pos2}; Colliding {colliding}");
}
Console.WriteLine($"Total time taken for {stepCount} steps: {stopWatch.ElapsedMilliseconds}ms, time per step{stopWatch.ElapsedMilliseconds / (float)stepCount}ms");
Console.ReadLine();
}
/// <summary>
/// Adds two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The sum of both vectors.</returns>
#region public static JVector operator +(JVector value1, JVector value2)
public static JVector operator +(JVector value1, JVector value2)
{
JVector result; JVector.Add(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Checks if given point is within a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref JMatrix orientation, ref JVector position, ref JVector point)
{
JVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
JVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
JVector r; support.SupportCenter(out r);
JVector.Transform(ref r, ref orientation, out r);
JVector.Add(ref position, ref r, out r);
JVector.Subtract(ref point, ref r, out r);
JVector x = point;
JVector w, p;
float VdotR;
JVector v; JVector.Subtract(ref x, ref arbitraryPoint, out v);
float dist = v.LengthSquared();
float epsilon = 0.0001f;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
JVector.Subtract(ref x, ref p, out w);
float VdotW = JVector.Dot(ref v, ref w);
if (VdotW > 0.0f)
{
VdotR = JVector.Dot(ref v, ref r);
if (VdotR >= -(JMath.Epsilon * JMath.Epsilon))
{
simplexSolverPool.GiveBack(simplexSolver); return(false);
}
else
{
simplexSolver.Reset();
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
dist = v.LengthSquared();
}
else
{
dist = 0.0f;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref JMatrix orientation, ref JMatrix invOrientation,
ref JVector position, ref JVector origin, ref JVector direction, out float fraction, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = JVector.Zero;
fraction = float.MaxValue;
float lambda = 0.0f;
JVector r = direction;
JVector x = origin;
JVector w, p, v;
JVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
JVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
float distSq = v.LengthSquared();
float epsilon = 0.000001f;
float VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
JVector.Subtract(ref x, ref p, out w);
float VdotW = JVector.Dot(ref v, ref w);
if (VdotW > 0.0f)
{
VdotR = JVector.Dot(ref v, ref r);
if (VdotR >= -JMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
JVector.Multiply(ref r, lambda, out x);
JVector.Add(ref origin, ref x, out x);
JVector.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
}
else
{
distSq = 0.0f;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
JVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.Length() / direction.Length();
#endregion
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Adds torque to the body. The torque gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the torque depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="torque">The torque to add next <see cref="World.Step"/>.</param>
public void AddTorque(JVector torque)
{
JVector.Add(ref torque, ref this.torque, out this.torque);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
public void AddForce(JVector force)
{
JVector.Add(ref force, ref this.force, out this.force);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
float deltaLength = dp.Length() - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
JVector n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
public LimitedHingeJoint(JitterWorld world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis,
float hingeFwdAngle, float hingeBckAngle)
: base(world)
{
// Create the hinge first, two point constraints
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
JVector pos1 = position; JVector.Add(ref pos1, ref hingeAxis, out pos1);
JVector pos2 = position; JVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
// Now the limit, one max distance constraint
hingeAxis.Normalize();
// choose a direction that is perpendicular to the hinge
JVector perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
// now make it perpendicular to the hinge
JVector sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * 3;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
JVector hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
JVector hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
JVector hingePos = body1.Position;
JVector relPos0c = hingePos + hingeRelAnchorPos0;
JVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance(body1, body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector point, out JVector normal, out float penetration)
{
// Used variables
JVector v01, v02, v0;
JVector v11, v12, v1;
JVector v21, v22, v2;
JVector v31, v32, v3;
JVector mn;
// Initialization of the output
point = normal = JVector.Zero;
penetration = 0.0f;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
JVector.Transform(ref v01, ref orientation1, out v01);
JVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
JVector.Transform(ref v02, ref orientation2, out v02);
JVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
JVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new JVector(0.00001f, 0);
}
// v1 = support in direction of origin
mn = v0;
JVector.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
JVector.Subtract(ref v12, ref v11, out v1);
if (JVector.Dot(ref v1, ref normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
normal = OutsidePortal(v1, v0);
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
JVector.Subtract(ref v22, ref v21, out v2);
//LD.Draw(Conversion.ToXNAVector2(v1), Conversion.ToXNAVector2(v0), Color.Blue);
//LD.Draw(Conversion.ToXNAVector2(v2), Conversion.ToXNAVector2(v0), Color.Blue);
if (JVector.Dot(ref v2, ref normal) <= 0.0f)
{
return(false);
}
// phase two: portal refinement
int maxIterations = 0;
while (true)
{
// find normal direction
if (!IntersectPortal(v0, v2, v1))
{
normal = InsidePortal(v2, v1);
}
else
{
// origin ray crosses the portal
normal = OutsidePortal(v2, v1);
}
// obtain the next support point
JVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
JVector.Subtract(ref v32, ref v31, out v3);
//LD.Draw(Conversion.ToXNAVector2(v3), Conversion.ToXNAVector2(v0), Color.Green);
if (JVector.Dot(v3, normal) <= 0)
{
JVector ab = v3 - v2;
float t = -(JVector.Dot(v2, ab)) / (JVector.Dot(ab, ab));
normal = (v2 + (t * ab));
return(false);
}
// Portal lies on the outside edge of the Minkowski Hull.
// Return contact information
if (JVector.Dot((v3 - v2), normal) <= CollideEpsilon || ++maxIterations > MaximumIterations)
{
JVector ab = v2 - v1;
float t = JVector.Dot(JVector.Negate(v1), ab);
if (t <= 0.0f)
{
t = 0.0f;
normal = v1;
}
else
{
float denom = JVector.Dot(ab, ab);
if (t >= denom)
{
normal = v2;
t = 1.0f;
}
else
{
t /= denom;
normal = v1 + t * ab;
}
}
float s = 1 - t;
point = s * v11 + t * v21;
var point2 = s * v12 + t * v22;
// this causes a sq root = bad!
penetration = normal.Length();
normal.Normalize();
return(true);
}
// if origin is inside (v1, v0, v3), refine portal
if (OriginInTriangle(v0, v1, v3))
{
v2 = v3;
v21 = v31;
v22 = v32;
continue;
}
// if origin is inside (v3, v0, v2), refine portal
else if (OriginInTriangle(v0, v2, v3))
{
v1 = v3;
v11 = v31;
v12 = v32;
continue;
}
return(false);
}
}
