/// <summary>
/// The points in world space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (body1IsParticle)
{
JVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
JMatrix xForm = JMatrix.CreateRotationZ(body1.invOrientation);
JVector.Transform(ref realRelPos1, ref xForm, out p1);
JVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsParticle)
{
JVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
JMatrix xForm = JMatrix.CreateRotationZ(body2.invOrientation);
JVector.Transform(ref realRelPos2, ref xForm, 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);
}
/// <summary>
/// Turn the <paramref name="input"/>-vector in the same direction as the <paramref name="direction"/>.
/// </summary>
/// <param name="input">Input vector to adjust</param>
/// <param name="direction">Given direction</param>
private void TurnIntoDirectionOf(ref JVector input, JVector direction)
{
if (JVector.Dot(input, direction) < 0.0f)
{
input = -1 * input;
}
}
public void SupportMapping(ref JVector direction, out JVector result)
{
float min = JVector.Dot(ref owner.points[indices.I0].position, ref direction);
float dot = JVector.Dot(ref owner.points[indices.I1].position, ref direction);
JVector minVertex = owner.points[indices.I0].position;
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I1].position;
}
dot = JVector.Dot(ref owner.points[indices.I2].position, ref direction);
if (dot > min)
{
min = dot;
minVertex = owner.points[indices.I2].position;
}
JVector exp;
JVector.Normalize(ref direction, out exp);
exp *= owner.triangleExpansion;
result = minVertex + exp;
}
/// <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>
/// 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.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
JVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
JVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
JVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
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);
}
/// <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 exp;
JVector.Normalize(ref direction, out exp);
exp *= sphericalExpansion;
float min = JVector.Dot(ref vecs[0], ref direction);
int minIndex = 0;
float dot = JVector.Dot(ref vecs[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = JVector.Dot(ref vecs[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
result = vecs[minIndex] + exp;
}
public override void Iterate()
{
JVector bodyLinearVelocity = this.Body1.LinearVelocity;
float lockedAxisMagnitude = JVector.Dot(lockedAxis, bodyLinearVelocity);
JVector correctionVelocity = lockedAxisMagnitude * lockedAxis;
Body1.LinearVelocity = bodyLinearVelocity - correctionVelocity;
}
/// <summary>
/// Reflect the <paramref name="input"/>-vector on the <paramref name="normal"/>-vector.
/// </summary>
/// <param name="input">Input-vector</param>
/// <param name="normal">Reflector-normal</param>
/// <returns>Vector which is the reflection of input on normal</returns>
private JVector ReflectOnNormal(JVector input, JVector normal)
{
JVector normalNormalized = normal;
normalNormalized.Normalize();
JVector inputNormalized = input;
inputNormalized.Normalize();
return(inputNormalized - 2 * JVector.Dot(inputNormalized, normalNormalized) * normalNormalized);
}
public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis, float hingeFwdAngle, float hingeBckAngle) : base(world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
var pos1 = position;
JVector.Add(pos1, hingeAxis, out pos1);
var pos2 = position;
JVector.Subtract(pos2, hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
hingeAxis = JVector.Normalize(hingeAxis);
var perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
var sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir = JVector.Normalize(perpDir);
float len = 10.0f * 3;
var hingeRelAnchorPos0 = perpDir * len;
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
var hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
var hingePos = body1.Position;
var relPos0c = hingePos + hingeRelAnchorPos0;
var relPos1c = hingePos + hingeRelAnchorPos1;
DistanceConstraint = new PointPointDistance(body1, body2, relPos0c, relPos1c)
{
Distance = allowedDistance,
Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance
};
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(JVector p, JVector a, JVector b, JVector c, JVector d)
{
JVector normal = JVector.Cross(b - a, c - a);
float signp = JVector.Dot(p - a, normal); // [AP AB AC]
float signd = JVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (1e-4f * 1e-4f))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < 0f ? 1 : 0);
}
/// <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);
}
}
private bool ShouldGenerateContact(RigidBody body, JVector[] triangle)
{
if (triangle == null)
{
return(true);
}
if (SurfaceTriangle.ComputeSurfaceType(triangle, WindingTypes.CounterClockwise) == SurfaceTypes.Floor)
{
return(false);
}
var n = Utilities.ComputeNormal(triangle[0], triangle[1], triangle[2], WindingTypes.CounterClockwise,
false);
return(JVector.Dot(controllingBody.LinearVelocity, n) < 0);
}
// This is primarily used for tracking relative rotation on moving platforms.
public static float ComputeYaw(this JMatrix matrix)
{
// See https://stackoverflow.com/a/4341489/7281613.
JVector t = JVector.Transform(JVector.Left, matrix);
JVector f = t - JVector.Dot(t, JVector.Up) * JVector.Up;
f.Normalize();
float angle = (float)Math.Acos(JVector.Dot(JVector.Left, f));
float d = JVector.Dot(JVector.Up, JVector.Cross(JVector.Left, f));
if (d < 0)
{
angle = Constants.TwoPi - angle;
}
return(angle);
}
/// <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 maxDotProduct = double.MinValue;
int maxIndex = 0;
double dotProduct;
for (int i = 0; i < vertices.Count; i++)
{
dotProduct = JVector.Dot(vertices[i], direction);
if (dotProduct > maxDotProduct)
{
maxDotProduct = dotProduct;
maxIndex = i;
}
}
result = vertices[maxIndex] - this.shifted;
}
private void HandlePlayerCollision(SteerableCollider player, Collider other, JVector collisionPoint, JVector normal)
{
//float angle = JVector.Dot(right, normal) > 0.0f ? -15.0f : 15.0f;
float angle = 0.0f;
angle += MathHelper.ToDegrees(player.RotationY);
JVector forward = Conversion.ToJitterVector(player.GameObject.transform.Forward);
// If the player is driving away of the wall there should be no collision-handling
if (JVector.Dot(forward, normal) > 0.0f)
{
return;
}
JVector newPosition = ProjectToNonCollision(player, collisionPoint, normal);
player.GameObject.GetComponent <Player>().SetCollisionState(other, Conversion.ToXnaVector(newPosition), angle);
player.Position = newPosition;
}
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);
}
internal static bool CircleCapsuleTest(JVector centerA, float radiusA, JVector centerB, JVector axis, float length, float radiusB, out JVector pointA, out JVector pointB, out JVector normal, out float distance)
{
// get capsule endpoints
var p0 = centerB - axis * (length * 0.5f);
var p1 = centerB + axis * (length * 0.5f);
// get vector from endpoint to circle
var D = centerA - p0;
// project vector onto axis and clamp
var d = JVector.Dot(D, axis);
d = JMath.Clamp(d, 0, length);
// get point on axis
var R = p0 + axis * d;
// distance
var b = Math.Abs((centerA - R).Length());
normal = (centerA - R) / b;
// calculate closest 2 points
var RH = JVector.Normalize(centerA - R);
pointA = JVector.Negate(RH) * radiusA + centerA;
pointB = RH * radiusB + R;
normal.Negate();
distance = b - (radiusA + radiusB);
//
if (b < radiusA + radiusB)
{
return(true);
}
return(false);
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
if (arbiter.contactList.Count == 0)
{
lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); }
return;
}
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact c = arbiter.contactList[i];
c.UpdatePosition();
if (c.penetration < -contactSettings.breakThreshold)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
else
{
JVector diff; JVector.Subtract(ref c.p1, ref c.p2, out diff);
double distance = JVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.LengthSquared();
// hack (multiplication by factor 100) in the
// following line.
if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
}
}
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(float timestep)
{
float dvx, dvy, dvz;
if (considerAngularVelocity)
{
dvx = (body2.angularVelocity.Y * relativePos2.Z) - (body2.angularVelocity.Z * relativePos2.Y) + body2.linearVelocity.X;
dvy = (body2.angularVelocity.Z * relativePos2.X) - (body2.angularVelocity.X * relativePos2.Z) + body2.linearVelocity.Y;
dvz = (body2.angularVelocity.X * relativePos2.Y) - (body2.angularVelocity.Y * relativePos2.X) + body2.linearVelocity.Z;
dvx = dvx - (body1.angularVelocity.Y * relativePos1.Z) + (body1.angularVelocity.Z * relativePos1.Y) - body1.linearVelocity.X;
dvy = dvy - (body1.angularVelocity.Z * relativePos1.X) + (body1.angularVelocity.X * relativePos1.Z) - body1.linearVelocity.Y;
dvz = dvz - (body1.angularVelocity.X * relativePos1.Y) + (body1.angularVelocity.Y * relativePos1.X) - body1.linearVelocity.Z;
}
else
{
dvx = body2.linearVelocity.X;
dvy = body2.linearVelocity.Y;
dvz = body2.linearVelocity.Z;
dvx = dvx - body1.linearVelocity.X;
dvy = dvy - body1.linearVelocity.Y;
dvz = dvz - body1.linearVelocity.Z;
}
float kNormal = 0.0f;
JVector rantra = JVector.Zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.X = (relativePos1.Y * normal.Z) - (relativePos1.Z * normal.Y);
rantra.Y = (relativePos1.Z * normal.X) - (relativePos1.X * normal.Z);
rantra.Z = (relativePos1.X * normal.Y) - (relativePos1.Y * normal.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rantra.X * body1.invInertiaWorld.M11) + (rantra.Y * body1.invInertiaWorld.M21)) + (rantra.Z * body1.invInertiaWorld.M31);
float num1 = ((rantra.X * body1.invInertiaWorld.M12) + (rantra.Y * body1.invInertiaWorld.M22)) + (rantra.Z * body1.invInertiaWorld.M32);
float num2 = ((rantra.X * body1.invInertiaWorld.M13) + (rantra.Y * body1.invInertiaWorld.M23)) + (rantra.Z * body1.invInertiaWorld.M33);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.Y * relativePos1.Z) - (rantra.Z * relativePos1.Y);
num1 = (rantra.Z * relativePos1.X) - (rantra.X * relativePos1.Z);
num2 = (rantra.X * relativePos1.Y) - (rantra.Y * relativePos1.X);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
}
}
JVector rbntrb = JVector.Zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.X = (relativePos2.Y * normal.Z) - (relativePos2.Z * normal.Y);
rbntrb.Y = (relativePos2.Z * normal.X) - (relativePos2.X * normal.Z);
rbntrb.Z = (relativePos2.X * normal.Y) - (relativePos2.Y * normal.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rbntrb.X * body2.invInertiaWorld.M11) + (rbntrb.Y * body2.invInertiaWorld.M21)) + (rbntrb.Z * body2.invInertiaWorld.M31);
float num1 = ((rbntrb.X * body2.invInertiaWorld.M12) + (rbntrb.Y * body2.invInertiaWorld.M22)) + (rbntrb.Z * body2.invInertiaWorld.M32);
float num2 = ((rbntrb.X * body2.invInertiaWorld.M13) + (rbntrb.Y * body2.invInertiaWorld.M23)) + (rbntrb.Z * body2.invInertiaWorld.M33);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.Y * relativePos2.Z) - (rbntrb.Z * relativePos2.Y);
num1 = (rbntrb.Z * relativePos2.X) - (rbntrb.X * relativePos2.Z);
num2 = (rbntrb.X * relativePos2.Y) - (rbntrb.Y * relativePos2.X);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
}
}
if (!treatBody1AsStatic)
{
kNormal += rantra.X * normal.X + rantra.Y * normal.Y + rantra.Z * normal.Z;
}
if (!treatBody2AsStatic)
{
kNormal += rbntrb.X * normal.X + rbntrb.Y * normal.Y + rbntrb.Z * normal.Z;
}
massNormal = 1.0f / kNormal;
float num = dvx * normal.X + dvy * normal.Y + dvz * normal.Z;
tangent.X = dvx - normal.X * num;
tangent.Y = dvy - normal.Y * num;
tangent.Z = dvz - normal.Z * num;
num = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;
if (num != 0.0f)
{
num = (float)Math.Sqrt(num);
tangent.X /= num;
tangent.Y /= num;
tangent.Z /= num;
}
float kTangent = 0.0f;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.X = (relativePos1.Y * tangent.Z) - (relativePos1.Z * tangent.Y);
rantra.Y = (relativePos1.Z * tangent.X) - (relativePos1.X * tangent.Z);
rantra.Z = (relativePos1.X * tangent.Y) - (relativePos1.Y * tangent.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rantra.X * body1.invInertiaWorld.M11) + (rantra.Y * body1.invInertiaWorld.M21)) + (rantra.Z * body1.invInertiaWorld.M31);
float num1 = ((rantra.X * body1.invInertiaWorld.M12) + (rantra.Y * body1.invInertiaWorld.M22)) + (rantra.Z * body1.invInertiaWorld.M32);
float num2 = ((rantra.X * body1.invInertiaWorld.M13) + (rantra.Y * body1.invInertiaWorld.M23)) + (rantra.Z * body1.invInertiaWorld.M33);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.Y * relativePos1.Z) - (rantra.Z * relativePos1.Y);
num1 = (rantra.Z * relativePos1.X) - (rantra.X * relativePos1.Z);
num2 = (rantra.X * relativePos1.Y) - (rantra.Y * relativePos1.X);
rantra.X = num0; rantra.Y = num1; rantra.Z = num2;
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.X = (relativePos2.Y * tangent.Z) - (relativePos2.Z * tangent.Y);
rbntrb.Y = (relativePos2.Z * tangent.X) - (relativePos2.X * tangent.Z);
rbntrb.Z = (relativePos2.X * tangent.Y) - (relativePos2.Y * tangent.X);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
float num0 = ((rbntrb.X * body2.invInertiaWorld.M11) + (rbntrb.Y * body2.invInertiaWorld.M21)) + (rbntrb.Z * body2.invInertiaWorld.M31);
float num1 = ((rbntrb.X * body2.invInertiaWorld.M12) + (rbntrb.Y * body2.invInertiaWorld.M22)) + (rbntrb.Z * body2.invInertiaWorld.M32);
float num2 = ((rbntrb.X * body2.invInertiaWorld.M13) + (rbntrb.Y * body2.invInertiaWorld.M23)) + (rbntrb.Z * body2.invInertiaWorld.M33);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.Y * relativePos2.Z) - (rbntrb.Z * relativePos2.Y);
num1 = (rbntrb.Z * relativePos2.X) - (rbntrb.X * relativePos2.Z);
num2 = (rbntrb.X * relativePos2.Y) - (rbntrb.Y * relativePos2.X);
rbntrb.X = num0; rbntrb.Y = num1; rbntrb.Z = num2;
}
}
if (!treatBody1AsStatic)
{
kTangent += JVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += JVector.Dot(ref rbntrb, ref tangent);
}
massTangent = 1.0f / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = 0.0f;
float relNormalVel = normal.X * dvx + normal.Y * dvy + normal.Z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (1.0f / timestep) * JMath.Max(0.0f, Penetration - settings.allowedPenetration);
restitutionBias = JMath.Clamp(restitutionBias, 0.0f, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
float timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
float relTangentVel = -(tangent.X * dvx + tangent.Y * dvy + tangent.Z * dvz);
float tangentImpulse = massTangent * relTangentVel;
float maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
JVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -1.0f && newContact)
{
restitutionBias = Math.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = 0.0f;
}
else
{
lostSpeculativeBounce = 0.0f;
}
impulse.X = normal.X * accumulatedNormalImpulse + tangent.X * accumulatedTangentImpulse;
impulse.Y = normal.Y * accumulatedNormalImpulse + tangent.Y * accumulatedTangentImpulse;
impulse.Z = normal.Z * accumulatedNormalImpulse + tangent.Z * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.X -= (impulse.X * body1.inverseMass);
body1.linearVelocity.Y -= (impulse.Y * body1.inverseMass);
body1.linearVelocity.Z -= (impulse.Z * body1.inverseMass);
if (!body1IsMassPoint && considerAngularVelocity)
{
float num0, num1, num2;
num0 = relativePos1.Y * impulse.Z - relativePos1.Z * impulse.Y;
num1 = relativePos1.Z * impulse.X - relativePos1.X * impulse.Z;
num2 = relativePos1.X * impulse.Y - relativePos1.Y * impulse.X;
float num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
float num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
float num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.X -= num3;
body1.angularVelocity.Y -= num4;
body1.angularVelocity.Z -= num5;
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.X += (impulse.X * body2.inverseMass);
body2.linearVelocity.Y += (impulse.Y * body2.inverseMass);
body2.linearVelocity.Z += (impulse.Z * body2.inverseMass);
if (!body2IsMassPoint && considerAngularVelocity)
{
float num0, num1, num2;
num0 = relativePos2.Y * impulse.Z - relativePos2.Z * impulse.Y;
num1 = relativePos2.Z * impulse.X - relativePos2.X * impulse.Z;
num2 = relativePos2.X * impulse.Y - relativePos2.Y * impulse.X;
float num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
float num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
float num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.X += num3;
body2.angularVelocity.Y += num4;
body2.angularVelocity.Z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// Calculates the dot product of two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>Returns the dot product of both.</returns>
#region public static Fix64 operator *(JVector value1, JVector value2)
public static Fix64 operator *(JVector value1, JVector value2)
{
return(JVector.Dot(ref value1, ref value2));
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(float timestep)
{
if (body1.IsTrigger || body2.IsTrigger)
{
return;
}
float dvx, dvy;
// relative velocity at contact point
dvx = body2.linearVelocity.X + (-body2.angularVelocity * relativePos2.Y);
dvy = body2.linearVelocity.Y + (body2.angularVelocity * relativePos2.X);
dvx = dvx - body1.linearVelocity.X + (-body1.angularVelocity * relativePos1.Y);
dvy = dvy - body1.linearVelocity.Y + (body1.angularVelocity * relativePos1.X);
float kNormal = 0.0f;
float rantra = 0.0f;
// if body1 isn't static
if (!treatBody1AsStatic)
{
// add it's mass to the mass normal
kNormal += body1.inverseMass;
// if body1 isn't a mass point (particle)
if (!body1IsMassPoint)
{
//
rantra = relativePos1.X * normal.Y - relativePos1.Y * normal.X;
}
}
float rbntrb = 0.0f;
// if body2 isn't static
if (!treatBody2AsStatic)
{
// add it's mass to the mass normal
kNormal += body2.inverseMass;
// if body1 isn't a mass point (particle)
if (!body2IsMassPoint)
{
//
rbntrb = relativePos2.X * normal.Y - relativePos2.Y * normal.X;
}
}
// compute overall mass normal
if (!treatBody1AsStatic)
{
kNormal += body1.invInertia * (rantra * rantra);
}
if (!treatBody2AsStatic)
{
kNormal += body2.invInertia * (rbntrb * rbntrb);
}
massNormal = 1.0f / kNormal;
tangent.X = -normal.Y;
tangent.Y = normal.X;
float kTangent = 0.0f;
if (treatBody1AsStatic)
{
rantra = 0.0f;
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
//
rantra = relativePos1.X * tangent.Y - relativePos1.Y * tangent.X;
}
}
if (treatBody2AsStatic)
{
rbntrb = 0.0f;
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
rantra = relativePos2.X * tangent.Y - relativePos2.Y * tangent.X;
}
}
// compute overall mass tangent
if (!treatBody1AsStatic)
{
kTangent += body1.invInertia * (rantra * rantra);
}
if (!treatBody2AsStatic)
{
kTangent += body2.invInertia * (rbntrb * rbntrb);
}
massTangent = 1.0f / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = 0.0f;
float relNormalVel = JVector.Dot(normal, body2.linearVelocity + JVector.Cross(body2.angularVelocity, relativePos2) - body1.linearVelocity + JVector.Cross(body1.angularVelocity, relativePos1));
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (1.0f / timestep) * JMath.Max(0.0f, Penetration - settings.allowedPenetration);
restitutionBias = JMath.Clamp(restitutionBias, 0.0f, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
float timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
float relTangentVel = -(tangent.X * dvx + tangent.Y * dvy);
float tangentImpulse = massTangent * relTangentVel;
float maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
JVector impulse;
// Simultaneous solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -1.0f && newContact)
{
restitutionBias = Math.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = 0.0f;
}
else
{
lostSpeculativeBounce = 0.0f;
}
// warm start
impulse.X = normal.X * accumulatedNormalImpulse + tangent.X * accumulatedTangentImpulse;
impulse.Y = normal.Y * accumulatedNormalImpulse + tangent.Y * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.X -= (impulse.X * body1.inverseMass);
body1.linearVelocity.Y -= (impulse.Y * body1.inverseMass);
if (!body1IsMassPoint)
{
body1.angularVelocity -= body1.invInertia * (relativePos1.X * impulse.Y - relativePos1.Y * impulse.X);
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.X += (impulse.X * body2.inverseMass);
body2.linearVelocity.Y += (impulse.Y * body2.inverseMass);
if (!body2IsMassPoint)
{
body2.angularVelocity += body2.invInertia * (relativePos2.X * impulse.Y - relativePos2.Y * impulse.X);
}
}
lastTimeStep = timestep;
newContact = false;
}
/// <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 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;
}
}
}
}
}
// 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);
}
public bool UpdateClosestVectorAndPoints()
{
if (_needsUpdate)
{
_cachedBC.Reset();
_needsUpdate = false;
JVector p, a, b, c, d;
switch (NumVertices)
{
case 0:
_cachedValidClosest = false;
break;
case 1:
_cachedPA = _simplexPointsP[0];
_cachedPB = _simplexPointsQ[0];
_cachedV = _cachedPA - _cachedPB;
_cachedBC.Reset();
_cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 2:
//closest point origin from line segment
JVector from = _simplexVectorW[0];
JVector to = _simplexVectorW[1];
JVector nearest;
JVector diff = from * (-1);
JVector v = to - from;
float t = JVector.Dot(v, diff);
if (t > 0)
{
float dotVV = v.LengthSquared();
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
_cachedBC.UsedVertices.UsedVertexA = true;
_cachedBC.UsedVertices.UsedVertexB = true;
}
else
{
t = 1;
diff -= v;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexB = true;
}
}
else
{
t = 0;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexA = true;
}
_cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
nearest = from + t * v;
_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 3:
//closest point origin from triangle
p = new JVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 4:
p = new JVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
d = _simplexVectorW[3];
bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
if (hasSeperation)
{
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
}
else
{
if (_cachedBC.Degenerate)
{
_cachedValidClosest = false;
}
else
{
_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
_cachedV.X = _cachedV.Y = _cachedV.Z = 0f;
}
break; // !!!!!!!!!!!! proverit na vsakiy sluchai
}
_cachedValidClosest = _cachedBC.IsValid;
//closest point origin from tetrahedron
break;
default:
_cachedValidClosest = false;
break;
}
}
return(_cachedValidClosest);
}
public void PreStep(float timeStep)
{
float vel = car.LinearVelocity.Length();
SideFriction = 2.5f - JMath.Clamp(vel / 20.0f, 0.0f, 1.4f);
ForwardFriction = 5.5f - JMath.Clamp(vel / 20.0f, 0.0f, 5.4f);
JVector force = JVector.Zero;
JVector worldAxis = JVector.Transform(JVector.Up, car.Orientation);
JVector worldPos = car.Position + JVector.Transform(Position, car.Orientation);
JVector forward = new JVector(-car.Orientation.M31, -car.Orientation.M32, -car.Orientation.M33);
JVector wheelFwd = JVector.Transform(forward, JMatrix.CreateFromAxisAngle(JVector.Up, SteerAngle / 360 * 2 * JMath.Pi));
JVector wheelLeft = JVector.Cross(JVector.Up, wheelFwd); wheelLeft.Normalize();
JVector wheelUp = JVector.Cross(wheelFwd, wheelLeft);
float rayLen = 2.0f * Radius + WheelTravel;
JVector wheelRayStart = worldPos;
JVector wheelDelta = -Radius * worldAxis;
JVector wheelRayEnd = worldPos + wheelDelta;
float deltaFwd = (2.0f * Radius) / (NumberOfRays + 1);
float deltaFwdStart = deltaFwd;
lastDisplacement = displacement;
displacement = 0.0f;
lastOnFloor = false;
JVector rayOrigin = car.Position + JVector.Transform(Position, car.Orientation);
JVector groundNormal = JVector.Zero;
JVector groundPos = JVector.Zero;
float deepestFrac = float.MaxValue;
RigidBody worldBody = null;
for (int i = 0; i < NumberOfRays; i++)
{
float distFwd = (deltaFwdStart + i * deltaFwd) - Radius;
float zOffset = Radius * (1.0f - (float)Math.Cos(Math.PI / 4 * (distFwd / Radius)));
JVector newOrigin = wheelRayStart + distFwd * wheelFwd + zOffset * wheelUp;
RigidBody body; JVector normal; float frac;
bool result = world.CollisionSystem.Raycast(newOrigin, wheelDelta,
raycast, out body, out normal, out frac);
if (result && frac <= 1.0f)
{
if (frac < deepestFrac)
{
deepestFrac = frac;
groundPos = rayOrigin + frac * wheelDelta;
worldBody = body;
groundNormal = normal;
}
lastOnFloor = true;
}
}
if (!lastOnFloor)
{
return;
}
if (groundNormal.LengthSquared() > 0.0f)
{
groundNormal.Normalize();
}
// System.Diagnostics.Debug.WriteLine(groundPos.ToString());
displacement = rayLen * (1.0f - deepestFrac);
displacement = JMath.Clamp(displacement, 0.0f, WheelTravel);
float displacementForceMag = displacement * Spring;
// reduce force when suspension is par to ground
displacementForceMag *= Math.Abs(JVector.Dot(groundNormal, worldAxis));
// apply damping
float dampingForceMag = upSpeed * Damping;
float totalForceMag = displacementForceMag + dampingForceMag;
if (totalForceMag < 0.0f)
{
totalForceMag = 0.0f;
}
JVector extraForce = totalForceMag * worldAxis;
force += extraForce;
JVector groundUp = groundNormal;
JVector groundLeft = JVector.Cross(groundNormal, wheelFwd);
if (groundLeft.LengthSquared() > 0.0f)
{
groundLeft.Normalize();
}
JVector groundFwd = JVector.Cross(groundLeft, groundUp);
JVector wheelPointVel = car.LinearVelocity +
JVector.Cross(car.AngularVelocity, JVector.Transform(Position, car.Orientation));
// rimVel=(wxr)*v
JVector rimVel = angVel * JVector.Cross(wheelLeft, groundPos - worldPos);
wheelPointVel += rimVel;
JVector worldVel = worldBody.LinearVelocity +
JVector.Cross(worldBody.AngularVelocity, groundPos - worldBody.Position);
wheelPointVel -= worldVel;
// sideways forces
float noslipVel = 0.1f;
float slipVel = 0.1f;
float slipFactor = 0.7f;
float smallVel = 3;
float friction = SideFriction;
float sideVel = JVector.Dot(wheelPointVel, groundLeft);
if ((sideVel > slipVel) || (sideVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((sideVel > noslipVel) || (sideVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(sideVel) - noslipVel) / (slipVel - noslipVel);
}
if (sideVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(sideVel) < smallVel)
{
friction *= System.Math.Abs(sideVel) / smallVel;
}
float sideForce = -friction * totalForceMag;
extraForce = sideForce * groundLeft;
force += extraForce;
// fwd/back forces
friction = ForwardFriction;
float fwdVel = JVector.Dot(wheelPointVel, groundFwd);
if ((fwdVel > slipVel) || (fwdVel < -slipVel))
{
friction *= slipFactor;
}
else
if ((fwdVel > noslipVel) || (fwdVel < -noslipVel))
{
friction *= 1.0f - (1.0f - slipFactor) * (System.Math.Abs(fwdVel) - noslipVel) / (slipVel - noslipVel);
}
if (fwdVel < 0.0f)
{
friction *= -1.0f;
}
if (System.Math.Abs(fwdVel) < smallVel)
{
friction *= System.Math.Abs(fwdVel) / smallVel;
}
float fwdForce = -friction * totalForceMag;
extraForce = fwdForce * groundFwd;
force += extraForce;
// fwd force also spins the wheel
JVector wheelCentreVel = car.LinearVelocity +
JVector.Cross(car.AngularVelocity, JVector.Transform(Position, car.Orientation));
angVelForGrip = JVector.Dot(wheelCentreVel, groundFwd) / Radius;
torque += -fwdForce * Radius;
// add force to car
car.AddForce(force, groundPos + 0.5f * JVector.Up);
// add force to the world
if (!worldBody.IsStatic)
{
worldBody.AddForce(force * (-1) * 0.01f, groundPos);
}
}
public bool ClosestPtPointTriangle(JVector p, JVector a, JVector b, JVector c,
ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
float v, w;
// Check if P in vertex region outside A
JVector ab = b - a;
JVector ac = c - a;
JVector ap = p - a;
float d1 = JVector.Dot(ab, ap);
float d2 = JVector.Dot(ac, ap);
if (d1 <= 0f && d2 <= 0f)
{
result.ClosestPointOnSimplex = a;
result.UsedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(1, 0, 0, 0);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
JVector bp = p - b;
float d3 = JVector.Dot(ab, bp);
float d4 = JVector.Dot(ac, bp);
if (d3 >= 0f && d4 <= d3)
{
result.ClosestPointOnSimplex = b;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(0, 1, 0, 0);
return(true); // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
float vc = d1 * d4 - d3 * d2;
if (vc <= 0f && d1 >= 0f && d3 <= 0f)
{
v = d1 / (d1 - d3);
result.ClosestPointOnSimplex = a + v * ab;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(1 - v, v, 0, 0);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
JVector cp = p - c;
float d5 = JVector.Dot(ab, cp);
float d6 = JVector.Dot(ac, cp);
if (d6 >= 0f && d5 <= d6)
{
result.ClosestPointOnSimplex = c;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 0, 1, 0);
return(true);//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
float vb = d5 * d2 - d1 * d6;
if (vb <= 0f && d2 >= 0f && d6 <= 0f)
{
w = d2 / (d2 - d6);
result.ClosestPointOnSimplex = a + w * ac;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - w, 0, w, 0);
return(true);
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
float va = d3 * d6 - d5 * d4;
if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
{
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.ClosestPointOnSimplex = b + w * (c - b);
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 1 - w, w, 0);
return(true);
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
float denom = 1.0f / (va + vb + vc);
v = vb * denom;
w = vc * denom;
result.ClosestPointOnSimplex = a + ab * v + ac * w;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
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 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);
}
/// <summary>
/// Project vector <paramref name="v"/> on vector <paramref name="u"/>
/// </summary>
/// <param name="v">Vector to project</param>
/// <param name="u">Vector to project on</param>
/// <returns>v projected on u</returns>
private JVector ProjectOn(JVector v, JVector u)
{
return(JVector.Dot(v, u) / u.LengthSquared() * u);
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
if (arbiter.contactList.Count == 0)
{
lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); }
return;
}
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact c = arbiter.contactList[i];
c.UpdatePosition();
if ((arbiter.body1.isStatic || arbiter.body2.isStatic) == false)
{
UnityEngine.Debug.Log(string.Format("Contacts {0}", arbiter.ContactList.Count));
}
if (c.penetration < -contactSettings.breakThreshold)
{
poolContact.GiveBack(c);
arbiter.contactList.RemoveAt(i);
if ((arbiter.body1.isStatic || arbiter.body2.isStatic) == false)
{
if (arbiter.ContactList.Count == 0)
{
UnityEngine.Debug.Log(string.Format("Removed all contacts through breakshold penetration"));
}
}
continue;
}
else
{
JVector diff; JVector.Subtract(ref c.p1, ref c.p2, out diff);
float distance = JVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.LengthSquared();
// hack (multiplication by factor 100) in the
// following line.
if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100)
{
poolContact.GiveBack(c);
arbiter.contactList.RemoveAt(i);
if ((arbiter.body1.isStatic || arbiter.body2.isStatic) == false)
{
if (arbiter.ContactList.Count == 0)
{
UnityEngine.Debug.Log(string.Format("Removed all contacts through distance"));
}
}
continue;
}
}
}
}