public void SupportCenter(out FPVector center)
{
center = owner.points[indices.I0].position;
FPVector.Add(ref center, ref owner.points[indices.I1].position, out center);
FPVector.Add(ref center, ref owner.points[indices.I2].position, out center);
FPVector.Multiply(ref center, FP.One / (3 * FP.One), out center);
}
/// <summary>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = FPMatrix.Zero;
this.invInertia = this.invInertiaWorld = FPMatrix.Zero;
this.invOrientation = this.orientation = FPMatrix.Identity;
this.boundingBox = shape.boundingBox;
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
FPMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
FPVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
FPVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
if (!isStatic)
{
FPMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
FPMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
/// <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 FPVector direction, out FPVector result)
{
FPVector expandVector;
FPVector.Normalize(ref direction, out expandVector);
FPVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = FPVector.Dot(ref points[0], ref direction);
FP dot = FPVector.Dot(ref points[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = FPVector.Dot(ref points[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
FPVector.Add(ref points[minIndex], ref expandVector, out result);
}
/// <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>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(FPVector force, FPVector pos)
{
FPVector.Add(ref this.force, ref force, out this.force);
FPVector.Subtract(ref pos, ref this.position, out pos);
FPVector.Cross(ref pos, ref force, out pos);
FPVector.Add(ref pos, ref this.torque, out this.torque);
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
FPVector temp;
FPVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
FPVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
FPVector.Multiply(ref body.torque, timestep, out temp);
FPVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
FPVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
}
if (body.affectedByGravity)
{
FPVector.Multiply(ref gravity, timestep, out temp);
FPVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
private void SupportMapping(RigidBody body, Shape workingShape, ref FPVector direction, out FPVector result)
{
FPVector.Transform(ref direction, ref body.invOrientation, out result);
workingShape.SupportMapping(ref result, out result);
FPVector.Transform(ref result, ref body.orientation, out result);
FPVector.Add(ref result, ref body.position, out result);
}
/// <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)
{
FPVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
FPVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
FPVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
FPVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
FPVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
FPVector.Add(ref p2, ref body2.position, out p2);
}
FPVector dist; FPVector.Subtract(ref p1, ref p2, out dist);
penetration = FPVector.Dot(ref dist, ref 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 FPVector direction, out FPVector result)
{
FPVector.Transform(ref direction, ref shapes[currentShape].invOrientation, out result);
shapes[currentShape].Shape.SupportMapping(ref direction, out result);
FPVector.Transform(ref result, ref shapes[currentShape].orientation, out result);
FPVector.Add(ref result, ref shapes[currentShape].position, out result);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
FPVector p1, p2, dp;
FPVector.Add(ref body1.position, ref r1, out p1);
FPVector.Add(ref body2.position, ref r2, out p2);
FPVector.Subtract(ref p2, ref p1, out dp);
FPVector l = FPVector.Transform(lineNormal, body1.orientation);
l.Normalize();
FPVector t = (p1 - p2) % l;
if (t.sqrMagnitude != FP.Zero)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -FP.One * t; // linearVel Body2
jacobian[3] = -FP.One * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = FP.One / effectiveMass;
}
bias = -(l % (p2 - p1)).magnitude * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <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(FPVector impulse)
{
if (this.isStatic)
{
return;
}
FPVector temp;
FPVector.Multiply(ref impulse, inverseMass, out temp);
FPVector.Add(ref linearVelocity, ref temp, out linearVelocity);
}
public override void SupportMapping(ref FPVector direction, out FPVector result)
{
FPVector temp1, temp2 = FPVector.zero;
for (int i = 0; i < shapes.Count; i++)
{
shapes[i].SupportMapping(ref direction, out temp1);
FPVector.Add(ref temp1, ref temp2, out temp2);
}
FPVector.Subtract(ref temp2, ref shifted, out result);
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
FPVector temp;
FPVector.Multiply(ref body.linearVelocity, timestep, out temp);
FPVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
FPVector axis;
FP angle = body.angularVelocity.magnitude;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
FPVector.Multiply(ref body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
FPVector.Multiply(ref body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
}
FPQuaternion dorn = new FPQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
FPQuaternion ornA; FPQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
FPQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize(); FPMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity *= 1 / (1 + timestep * body.linearDrag);
body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* FPVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* FPVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
FPVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
FPVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
FPVector p1, p2, dp;
FPVector.Add(ref body1.position, ref r1, out p1);
FPVector.Add(ref body2.position, ref r2, out p2);
FPVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude;
FPVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ FPVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ FPVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += FPVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <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(FPVector impulse, FPVector relativePosition)
{
if (this.isStatic)
{
return;
}
FPVector temp;
FPVector.Multiply(ref impulse, inverseMass, out temp);
FPVector.Add(ref linearVelocity, ref temp, out linearVelocity);
FPVector.Cross(ref relativePosition, ref impulse, out temp);
FPVector.Transform(ref temp, ref invInertiaWorld, out temp);
FPVector.Add(ref angularVelocity, ref temp, out angularVelocity);
}
/// <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 FPBBox aabb, EChild child, out FPBBox result)
{
FPVector dims;
FPVector.Subtract(ref aabb.max, ref aabb.min, out dims);
FPVector.Multiply(ref dims, FP.Half, out dims);
FPVector offset = FPVector.zero;
switch (child)
{
case EChild.PPP: offset = new FPVector(1, 1, 1); break;
case EChild.PPM: offset = new FPVector(1, 1, 0); break;
case EChild.PMP: offset = new FPVector(1, 0, 1); break;
case EChild.PMM: offset = new FPVector(1, 0, 0); break;
case EChild.MPP: offset = new FPVector(0, 1, 1); break;
case EChild.MPM: offset = new FPVector(0, 1, 0); break;
case EChild.MMP: offset = new FPVector(0, 0, 1); break;
case EChild.MMM: offset = new FPVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new FPBBox();
result.min = new FPVector(offset.x * dims.x, offset.y * dims.y, offset.z * dims.z);
FPVector.Add(ref result.min, ref aabb.min, out result.min);
FPVector.Add(ref result.min, ref dims, out result.max);
// expand it just a tiny bit just to be safe!
FP extra = FP.EN5;
FPVector temp; FPVector.Multiply(ref dims, extra, out temp);
FPVector.Subtract(ref result.min, ref temp, out result.min);
FPVector.Add(ref result.max, ref temp, out result.max);
}
private HingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, FPVector position, FPVector hingeAxis) : base((World)world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
FPVector pos1 = position; FPVector.Add(ref pos1, ref hingeAxis, out pos1);
FPVector pos2 = position; FPVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos1);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
Activate();
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = FPVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += FPVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = FPMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = FPMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
FPVector temp;
if (!body1.isStatic)
{
FPVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
FPVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
if (!body2.isStatic)
{
FPVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
FPVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
}
public override void MakeHull(ref List <FPVector> triangleList, int generationThreshold)
{
List <FPVector> triangles = new List <FPVector>();
for (int i = 0; i < shapes.Length; i++)
{
shapes[i].Shape.MakeHull(ref triangles, 4);
for (int e = 0; e < triangles.Count; e++)
{
FPVector pos = triangles[e];
FPVector.Transform(ref pos, ref shapes[i].orientation, out pos);
FPVector.Add(ref pos, ref shapes[i].position, out pos);
triangleList.Add(pos);
}
triangles.Clear();
}
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref FPVector point, ref FPVector normal,
out FPVector point1, out FPVector point2)
{
FPVector mn; FPVector.Negate(ref normal, out mn);
FPVector sA; SupportMapping(body1, shape1, ref mn, out sA);
FPVector sB; SupportMapping(body2, shape2, ref normal, out sB);
FPVector.Subtract(ref sA, ref point, out sA);
FPVector.Subtract(ref sB, ref point, out sB);
FP dot1 = FPVector.Dot(ref sA, ref normal);
FP dot2 = FPVector.Dot(ref sB, ref normal);
FPVector.Multiply(ref normal, dot1, out sA);
FPVector.Multiply(ref normal, dot2, out sB);
FPVector.Add(ref point, ref sA, out point1);
FPVector.Add(ref point, ref sB, out point2);
}
public void DebugDraw(IDebugDrawer drawer)
{
FPVector pos1, pos2, pos3;
for (int i = 0; i < hullPoints.Count; i += 3)
{
pos1 = hullPoints[i + 0];
pos2 = hullPoints[i + 1];
pos3 = hullPoints[i + 2];
FPVector.Transform(ref pos1, ref orientation, out pos1);
FPVector.Add(ref pos1, ref position, out pos1);
FPVector.Transform(ref pos2, ref orientation, out pos2);
FPVector.Add(ref pos2, ref position, out pos2);
FPVector.Transform(ref pos3, ref orientation, out pos3);
FPVector.Add(ref pos3, ref position, out pos3);
drawer.DrawTriangle(pos1, pos2, pos3);
}
}
/// <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);
}
FPVector sum = points[0];
FPVector.Add(ref sum, ref points[1], out sum);
FPVector.Add(ref sum, ref points[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref points[1], ref points[0], out sum);
FPVector.Subtract(ref points[2], ref points[0], out normal);
FPVector.Cross(ref sum, ref normal, out normal);
}
/// <summary>
/// Transforms the bounding box into the space given by orientation and position.
/// </summary>
/// <param name="position"></param>
/// <param name="orientation"></param>
/// <param name="result"></param>
internal void InverseTransform(ref FPVector position, ref FPMatrix orientation)
{
FPVector.Subtract(ref max, ref position, out max);
FPVector.Subtract(ref min, ref position, out min);
FPVector center;
FPVector.Add(ref max, ref min, out center);
center.x *= FP.Half; center.y *= FP.Half; center.z *= FP.Half;
FPVector halfExtents;
FPVector.Subtract(ref max, ref min, out halfExtents);
halfExtents.x *= FP.Half; halfExtents.y *= FP.Half; halfExtents.z *= FP.Half;
FPVector.TransposedTransform(ref center, ref orientation, out center);
FPMatrix abs; FPMath.Absolute(ref orientation, out abs);
FPVector.TransposedTransform(ref halfExtents, ref abs, out halfExtents);
FPVector.Add(ref center, ref halfExtents, out max);
FPVector.Subtract(ref center, ref halfExtents, out min);
}
/// <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);
FPVector sum = vecs[0];
FPVector.Add(ref sum, ref vecs[1], out sum);
FPVector.Add(ref sum, ref vecs[2], out sum);
FPVector.Multiply(ref sum, FP.One / (3 * FP.One), out sum);
geomCen = sum;
FPVector.Subtract(ref vecs[1], ref vecs[0], out sum);
FPVector.Subtract(ref vecs[2], ref vecs[0], out normal);
FPVector.Cross(ref sum, ref normal, out normal);
if (flipNormal)
{
normal.Negate();
}
}
/// <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 AddRelativeTorque(FPVector torque)
{
torque = FPVector.Transform(torque, this.FPOrientation);
FPVector.Add(ref torque, ref this.torque, out this.torque);
}
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
//ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB,
//out JVector p1, out JVector p2, out JVector normal)
// {
// VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
// simplexSolver.Reset();
// FP lambda = FP.Zero;
// p1 = p2 = JVector.Zero;
// JVector x1 = position1;
// JVector x2 = position2;
// JVector r = sweptA - sweptB;
// JVector w, v;
// JVector supVertexA;
// JVector rn = JVector.Negate(r);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA);
// JVector supVertexB;
// SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB);
// v = supVertexA - supVertexB;
// bool hasResult = false;
// normal = JVector.Zero;
// FP lastLambda = lambda;
// int maxIter = MaxIterations;
// FP distSq = v.LengthSquared();
// FP epsilon = FP.EN5;
// FP VdotR;
// while ((distSq > epsilon) && (maxIter-- != 0))
// {
// JVector vn = JVector.Negate(v);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA);
// SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB);
// w = supVertexA - supVertexB;
// FP VdotW = JVector.Dot(ref v, ref w);
// if (VdotW > FP.Zero)
// {
// VdotR = JVector.Dot(ref v, ref r);
// if (VdotR >= -JMath.Epsilon)
// {
// simplexSolverPool.GiveBack(simplexSolver);
// return false;
// }
// else
// {
// lambda = lambda - VdotW / VdotR;
// x1 = position1 + lambda * sweptA;
// x2 = position2 + lambda * sweptB;
// w = supVertexA - supVertexB;
// normal = v;
// hasResult = true;
// }
// }
// if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
// if (simplexSolver.Closest(out v))
// {
// distSq = v.LengthSquared();
// normal = v;
// hasResult = true;
// }
// else distSq = FP.Zero;
// }
// simplexSolver.ComputePoints(out p1, out p2);
// if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
// normal.Normalize();
// p1 = p1 - lambda * sweptA;
// p2 = p2 - lambda * sweptB;
// simplexSolverPool.GiveBack(simplexSolver);
// return true;
// }
#endregion
// 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 FPMatrix orientation, ref FPMatrix invOrientation,
ref FPVector position, ref FPVector origin, ref FPVector direction, out FP fraction, out FPVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = FPVector.zero;
fraction = FP.MaxValue;
FP lambda = FP.Zero;
FPVector r = direction;
FPVector x = origin;
FPVector w, p, v;
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
FPVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = FP.EN6;
FP VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -FPMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
FPVector.Multiply(ref r, lambda, out x);
FPVector.Add(ref origin, ref x, out x);
FPVector.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.sqrMagnitude;
}
else
{
distSq = FP.Zero;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
FPVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.magnitude / direction.magnitude;
#endregion
if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <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 FPMatrix orientation, ref FPVector position, ref FPVector point)
{
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
FPVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
FPVector r; support.SupportCenter(out r);
FPVector.Transform(ref r, ref orientation, out r);
FPVector.Add(ref position, ref r, out r);
FPVector.Subtract(ref point, ref r, out r);
FPVector x = point;
FPVector w, p;
FP VdotR;
FPVector v; FPVector.Subtract(ref x, ref arbitraryPoint, out v);
FP dist = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -(FPMath.Epsilon * FPMath.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.sqrMagnitude;
}
else
{
dist = FP.Zero;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <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(FPVector force)
{
FPVector.Add(ref force, ref this.force, out this.force);
}
/// <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(FPVector torque)
{
FPVector.Add(ref torque, ref this.torque, out this.torque);
}
/// <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 FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector point, out FPVector normal, out FP penetration)
{
// Used variables
FPVector temp1, temp2;
FPVector v01, v02, v0;
FPVector v11, v12, v1;
FPVector v21, v22, v2;
FPVector v31, v32, v3;
FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero;
FPVector mn;
// Initialization of the output
point = normal = FPVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
FPVector.Transform(ref v01, ref orientation1, out v01);
FPVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
FPVector.Transform(ref v02, ref orientation2, out v02);
FPVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
FPVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new FPVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
FPVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
FPVector.Subtract(ref v12, ref v11, out v1);
if (FPVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
FPVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
FPVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
FPVector.Add(ref point, ref v12, out point);
FPVector.Multiply(ref point, FP.Half, out point);
FPVector.Subtract(ref v12, ref v11, out temp1);
penetration = FPVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
FPVector.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);
FPVector.Subtract(ref v22, ref v21, out v2);
if (FPVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
FP dist = FPVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
FPVector.Swap(ref v1, ref v2);
FPVector.Swap(ref v11, ref v21);
FPVector.Swap(ref v12, ref v22);
FPVector.Negate(ref normal, out normal);
UnityEngine.Debug.Log("normal: " + 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
FPVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
FPVector.Subtract(ref v32, ref v31, out v3);
if (FPVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
FPVector.Cross(ref v1, ref v3, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v3, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
FPVector.Cross(ref v3, ref v2, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
FPVector.Subtract(ref v3, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
FPVector.Subtract(ref v2, ref v1, out temp1);
FPVector.Subtract(ref v3, ref v1, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = FPVector.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
FPVector.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);
FPVector.Subtract(ref v42, ref v41, out v4);
FPVector.Subtract(ref v4, ref v3, out temp1);
FP delta = FPVector.Dot(ref temp1, ref normal);
penetration = FPVector.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 <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
FPVector.Cross(ref v1, ref v2, out temp1);
FP b0 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v3, ref v2, out temp1);
FP b1 = FPVector.Dot(ref temp1, ref v0);
FPVector.Cross(ref v0, ref v1, out temp1);
FP b2 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v2, ref v1, out temp1);
FP b3 = FPVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
FPVector.Cross(ref v2, ref v3, out temp1);
b1 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v3, ref v1, out temp1);
b2 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v1, ref v2, out temp1);
b3 = FPVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
FPVector.Multiply(ref v01, b0, out point);
FPVector.Multiply(ref v11, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v21, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v31, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v02, b0, out temp2);
FPVector.Add(ref temp2, ref point, out point);
FPVector.Multiply(ref v12, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v22, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v32, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref point, inv * FP.Half, 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);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
FPVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = FPVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = FPVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = FPVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}