public void Rope()
{
RigidBody last = null;
for (int i = 0; i < RopeSize; i++)
{
RigidBody body = new RigidBody(new BoxShape(JVector.One));
body.Position = new JVector(i * 1.5f, 0.5f, 0);
JVector jpos2 = body.Position;
body.Position = jpos2;
world.AddBody(body);
body.Update();
if (last != null)
{
JVector jpos3 = last.Position;
JVector dif; JVector.Subtract(ref jpos2, ref jpos3, out dif);
JVector.Multiply(ref dif, 0.5f, out dif);
JVector.Subtract(ref jpos2, ref dif, out dif);
Constraint cons = new PointConstraint(last, body, dif);
world.AddConstraint(cons);
}
last = body;
}
}
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.isParticle))
{
// pretty sure this could be more complex ... lol
body.orientation += body.angularVelocity * timestep;
}
if ((body.Damping & RigidBody.DampingType.Linear) != 0)
{
JVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
}
if ((body.Damping & RigidBody.DampingType.Angular) != 0)
{
body.angularVelocity *= currentAngularDampFactor;
}
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
public override void Build()
{
AddGround();
RigidBody last = null;
for (int i = 0; i < 12; i++)
{
var body = new RigidBody(new BoxShape(JVector.One))
{
Position = new JVector((i * 1.5f) - 20, 0.5f, 0)
};
var jpos2 = body.Position;
Demo.World.AddBody(body);
body.Update();
if (last != null)
{
var jpos3 = last.Position;
JVector.Subtract(ref jpos2, ref jpos3, out var dif);
JVector.Multiply(ref dif, 0.5f, out dif);
JVector.Subtract(ref jpos2, ref dif, out dif);
Constraint cons = new PointOnPoint(last, body, dif);
Demo.World.AddConstraint(cons);
}
last = body;
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
result = direction;
result.Normalize();
JVector.Multiply(ref result, radius, out result);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
JVector expandVector;
JVector.Normalize(ref direction, out expandVector);
JVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
float min = JVector.Dot(ref points[0], ref direction);
float dot = JVector.Dot(ref points[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = JVector.Dot(ref points[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
JVector.Add(ref points[minIndex], ref expandVector, out result);
}
/// <summary>
/// Multiplies a vector by a scale factor.
/// </summary>
/// <param name="value2">The vector to scale.</param>
/// <param name="value1">The scale factor.</param>
/// <returns>Returns the scaled vector.</returns>
#region public static JVector operator *(Fix64 value1, JVector value2)
public static JVector operator *(Fix64 value1, JVector value2)
{
JVector result;
JVector.Multiply(ref value2, value1, out result);
return(result);
}
/// <summary>
/// Multiply a vector with a factor.
/// </summary>
/// <param name="value1">The vector to multiply.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <returns>Returns the multiplied vector.</returns>
#region public static JVector Multiply(JVector value1, Fix64 scaleFactor)
public static JVector Multiply(JVector value1, Fix64 scaleFactor)
{
JVector result;
JVector.Multiply(ref value1, scaleFactor, out result);
return(result);
}
private void IntegrateForces()
{
foreach (RigidBody body in rigidBodies)
{
if (!body.isStatic && body.IsActive)
{
JVector temp;
JVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
JVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
JVector.Multiply(ref body.torque, timestep, out temp);
JVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
JVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
}
if (body.affectedByGravity)
{
JVector.Multiply(ref gravity, timestep, out temp);
JVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
public void SupportCenter(out JVector center)
{
center = owner.points[indices.I0].position;
JVector.Add(ref center, ref owner.points[indices.I1].position, out center);
JVector.Add(ref center, ref owner.points[indices.I2].position, out center);
JVector.Multiply(ref center, 1.0f / 3.0f, out center);
}
/// <summary>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(JVector impulse)
{
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);
}
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.isParticle))
{
//exponential map
JVector axis;
double 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, ((double)Math.Sin(0.5f * angle * timestep) / angle), out axis);
}
JQuaternion dorn = new JQuaternion(axis.X, axis.Y, axis.Z, (double)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>
/// 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);
}
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>
/// 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);
}
/// <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>
/// 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);
}
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>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
// this Sqrt is bad!
float r = (float)Math.Sqrt(direction.X * direction.X);
if (Math.Abs(direction.Y) > 0.0f)
{
JVector dir; JVector.Normalize(ref direction, out dir);
JVector.Multiply(ref dir, radius, out result);
result.Y += Math.Sign(direction.Y) * 0.5f * length;
}
else if (r > 0.0f)
{
result.X = direction.X / r * radius;
result.Y = 0.0f;
}
else
{
result.X = 0.0f;
result.Y = 0.0f;
}
}
public override void Update(uint deltaMs)
{
var entities = OwnerApp.EntityManager.WithComponent <MovableComponent>().ToArray();
foreach (var entity in entities)
{
var ridgidBodyComponent = OwnerApp.EntityManager.GetComponent <RigidBodyComponent>(entity);
var movableComponent = OwnerApp.EntityManager.GetComponent <MovableComponent>(entity);
var velocity = JVector.Multiply(CalcVelocity(movableComponent.Speed), deltaMs / 16.6f);
if (velocity != JVector.Zero && !(ridgidBodyComponent.Body is null))
{
ridgidBodyComponent.Body.ApplyImpulse(velocity);
}
if (Keyboard.GetState().IsKeyDown(Key.Space)) // TODO && !ridgidBodyComponent.Body.IsActive)
{
ridgidBodyComponent.Body.ApplyImpulse(new JVector(0.0f, movableComponent.JumpSpeed, 0.0f));
}
}
}
/// <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);
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref JVector direction, out JVector result)
{
double r = (double)Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z);
if (Math.Abs(direction.Y) > 0.0f)
{
JVector dir; JVector.Normalize(ref direction, out dir);
JVector.Multiply(ref dir, radius, out result);
result.Y += Math.Sign(direction.Y) * 0.5f * length;
}
else if (r > 0.0f)
{
result.X = direction.X / r * radius;
result.Y = 0.0f;
result.Z = direction.Z / r * radius;
}
else
{
result.X = 0.0f;
result.Y = 0.0f;
result.Z = 0.0f;
}
}
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);
}
// 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>
/// 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;
}
}
}
}
}
