/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
double jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1];
double softnessScalar = accumulatedImpulse * softnessOverDt;
double lambda = -effectiveMass * (jv + bias + softnessScalar);
accumulatedImpulse += lambda;
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += JVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
/// Gets up to two supports points if the given direction is within a tolerance of being parallel to the normal of the edge formed by the supports. Otherwise it just returns a single support.
/// </summary>
internal int FindSupportPoints(ref JVector direction, ref JVector PA, ref JMatrix OA, out JVector[] S)
{
// init S
S = new JVector[2];
// transform the normal into object space
JVector N = JVector.TransposedTransform(direction, OA);
// find dots
float a = this.GetCorner(0) * N;
float b = this.GetCorner(1) * N;
float c = this.GetCorner(2) * N;
float d = this.GetCorner(3) * N;
// find min
float min = JMath.Min(a, b);
min = JMath.Min(c, min);
min = JMath.Min(d, min);
// now min should be right
int Snum = 0;
const float threshold = 1.0E-3f;
if (a < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(0), OA) + PA;
}
if (b < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(1), OA) + PA;
}
if (c < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(2), OA) + PA;
}
if (d < min + threshold)
{
S[Snum++] = JVector.Transform(this.GetCorner(3), OA) + PA;
}
return(Snum);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
float jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1];
float softnessScalar = accumulatedImpulse * softnessOverDt;
float lambda = -effectiveMass * (jv + bias + softnessScalar);
accumulatedImpulse += lambda;
// CUSTOM: Modified to use the IsStatic property.
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += JVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector p1, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
JVector l = lineNormal;
JVector t = (p1 - anchor) % l;
if (t.LengthSquared() != 0.0f)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t;
jacobian[1] = r1 % t;
effectiveMass = body1.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p1 - anchor)).Length() * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property.
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
JVector jv = body1.angularVelocity - body2.angularVelocity;
JVector softnessVector = accumulatedImpulse * softnessOverDt;
JVector lambda = -1.0f * JVector.Transform(jv + bias + softnessVector, effectiveMass);
accumulatedImpulse += lambda;
if (!body1.IsStatic)
{
body1.angularVelocity += JVector.Transform(lambda, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += JVector.Transform(-1.0f * lambda, body2.invInertiaWorld);
}
}
/// <summary>
///
/// </summary>
/// <param name="timeStep"></param>
public override void PreStep(float timeStep)
{
float damping = (float)Math.Pow(Damping, timeStep);
foreach (RigidBody body in bodies)
{
if (FluidBox.Contains(body.BoundingBox) != JBBox.ContainmentType.Disjoint)
{
JVector[] positions = samples[body.Shape];
float frac = 0.0f;
JVector currentCoord = JVector.Zero;
for (int i = 0; i < positions.Length; i++)
{
currentCoord = JVector.Transform(positions[i], body.Orientation);
currentCoord = JVector.Add(currentCoord, body.Position);
bool containsCoord = false;
if (fluidArea == null)
{
containsCoord = FluidBox.Contains(ref currentCoord) != JBBox.ContainmentType.Disjoint;
}
else
{
containsCoord = fluidArea(ref currentCoord);
}
if (containsCoord)
{
body.AddForce((1.0f / positions.Length) * body.Mass * Flow);
body.AddForce(-(1.0f / positions.Length) * body.Shape.Mass * Density * world.Gravity, currentCoord);
frac += 1.0f / positions.Length;
}
}
body.AngularVelocity *= damping;
body.LinearVelocity *= damping;
}
}
}
public CompoundShape(List <TransformedShape> shapes)
{
this.shapes = shapes.ToArray();
var jshapes = new List <Jitter.Collision.Shapes.CompoundShape.TransformedShape>();
// Remove original reference model from scene graph
foreach (var shape in shapes)
{
// TODO: Decompose - for now we only have rotations and translations so this should work
JMatrix orientation = Util.ToJitter(shape.Transform);
Vector3 p = shape.Transform.GetColumn3(3);
JVector position = Util.ToJitter(p);
Jitter.Collision.Shapes.Shape oldShape = shape.Shape.shape;
Jitter.Collision.Shapes.CompoundShape compound = oldShape as Jitter.Collision.Shapes.CompoundShape;
if (compound != null)
{
foreach (var part in compound.Shapes)
{
var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape(
part.Shape,
JMatrix.Multiply(orientation, part.Orientation),
position + JVector.Transform(part.Position, orientation)
);
jshapes.Add(newShape);
}
}
else
{
var newShape = new Jitter.Collision.Shapes.CompoundShape.TransformedShape(
oldShape,
orientation,
position
);
jshapes.Add(newShape);
}
}
compoundShape = new Jitter.Collision.Shapes.CompoundShape(jshapes);
Matrix4 vertexOffset = Matrix4.CreateTranslation(compoundShape.Shift.X, compoundShape.Shift.Y, compoundShape.Shift.Z);
Matrix4 modelOffset = Matrix4.CreateTranslation(-compoundShape.Shift.X, -compoundShape.Shift.Y, -compoundShape.Shift.Z);
}
/// <summary>
/// Functionality which is applied after the physics is updated
/// </summary>
/// <param name="timestep"></param>
public override void PostStep(float timestep)
{
if (!IsStatic && !IsAnimated)
{
GameObject.transform.position = Conversion.ToXnaVector(Position - JVector.Transform(CenterOfMass, Orientation));
GameObject.transform.rotation = Conversion.ToXnaQuaternion(JQuaternion.CreateFromMatrix(Orientation));
_newRotation = GameObject.transform.eulerAngles.Y;
//float test = (float)Math.Acos(Vector3.Dot(GameObject.transform.Forward, Vector3.Forward));
//Debug.Log(test + " == " + _newRotation);
}
if (SyncedObjects.Count > 0)
{
Vector3 projectedToGround = GameObject.transform.position;
projectedToGround.Y = 0;
float rotation = GameObject.transform.eulerAngles.Y;
foreach (Follower follower in SyncedObjects)
{
switch (follower.Type)
{
case Follower.FollowingType.OnFloor:
float offset = MathHelper.Clamp(GameObject.transform.position.Y, 1, 10);
follower.GameObject.transform.rotation = Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(rotation));
follower.GameObject.transform.position = projectedToGround + offset * follower.Offset;
break;
case Follower.FollowingType.Orientated:
var tmp = Quaternion.CreateFromAxisAngle(Vector3.Up, MathHelper.ToRadians(rotation));
follower.GameObject.transform.rotation = tmp * follower.Orientation;
follower.GameObject.transform.position = projectedToGround + Vector3.Transform(follower.Offset, tmp);
break;
}
}
}
base.PostStep(timestep);
}
public void TransformToBody()
{
var position = transform.position.ToJVector();
var orientation = transform.rotation.ToJMatrix();
var compound = Shape as CompoundShape;
if (compound != null)
{
position -= JVector.Transform(compound.Shift, orientation);
}
Body.Position = position;
Body.Orientation = orientation;
//if (Body.IsActive == false && Body.IsStatic == false)
//{
// var island = JPhysics.World.Islands.First(i => i.Bodies.Contains(Body));
// foreach (RigidBody b in island.Bodies)
// b.IsActive = true;
//}
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape. (Inlcuding all
/// 'sub' shapes)
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref JMatrix orientation, out JBBox box)
{
box.Min = mInternalBBox.Min;
box.Max = mInternalBBox.Max;
JVector localHalfExtents = 0.5f * (box.Max - box.Min);
JVector localCenter = 0.5f * (box.Max + box.Min);
JVector center;
JVector.Transform(ref localCenter, ref orientation, out center);
JMatrix abs; JMath.Absolute(ref orientation, out abs);
JVector temp;
JVector.Transform(ref localHalfExtents, ref abs, out temp);
box.Max = center + temp;
box.Min = center - temp;
}
/// <summary>
/// Add rigid-body to the draw-list to draw on the screen.
/// Only rigid-bodies with "Tag" = "DrawMe" are drawn.
/// </summary>
/// <param name="rb">Rigidbody</param>
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && (BodyTag)rb.Tag == BodyTag.DontDrawMe)
{
return;
}
Collider c = rb as Collider;
if (c?.GameObject.tag == ObjectTag.Ground)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
public void DebugDraw(IDebugDrawer drawer)
{
JVector pos1, pos2, pos3;
for (int i = 0; i < hullPoints.Count; i += 3)
{
pos1 = hullPoints[i + 0];
pos2 = hullPoints[i + 1];
pos3 = hullPoints[i + 2];
JVector.Transform(ref pos1, ref orientation, out pos1);
JVector.Add(ref pos1, ref position, out pos1);
JVector.Transform(ref pos2, ref orientation, out pos2);
JVector.Add(ref pos2, ref position, out pos2);
JVector.Transform(ref pos3, ref orientation, out pos3);
JVector.Add(ref pos3, ref position, out pos3);
drawer.DrawTriangle(pos1, pos2, pos3);
}
}
/// <summary>
/// The points in wolrd space gets recalculated by transforming the
/// local coordinates. Also new penetration depth is estimated.
/// </summary>
public void UpdatePosition()
{
if (!body1IsMassPoint)
{
JVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
}
JVector.Add(ref p1, ref body1.position, out p1);
if (!body2IsMassPoint)
{
JVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
}
JVector.Add(ref p2, ref body2.position, out p2);
JVector dist; JVector.Subtract(ref p1, ref p2, out dist);
penetration = JVector.Dot(ref dist, ref normal);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
effectiveMass = body1.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix q = JMatrix.Transpose(orientation) * body1.orientation;
JVector axis;
float x = q.M32 - q.M23;
float y = q.M13 - q.M31;
float z = q.M21 - q.M12;
float r = JMath.Sqrt(x * x + y * y + z * z);
float t = q.M11 + q.M22 + q.M33;
float angle = (float)Math.Atan2(r, t - 1);
axis = new JVector(x, y, z) * angle;
if (r != 0.0f)
{
axis = axis * (1.0f / r);
}
bias = axis * biasFactor * (-1.0f / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += JVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
}
private void AddBodyToDrawList(RigidBody rb)
{
if (rb.Tag is BodyTag && ((BodyTag)rb.Tag) == BodyTag.DontDrawMe)
{
return;
}
bool isCompoundShape = (rb.Shape is CompoundShape);
if (!isCompoundShape)
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.Default;
AddShapeToDrawList(rb.Shape, rb.Orientation, rb.Position);
}
else
{
//GraphicsDevice.BlendState = BlendState.Opaque;
//GraphicsDevice.DepthStencilState = DepthStencilState.None;
CompoundShape cShape = rb.Shape as CompoundShape;
JMatrix orientation = rb.Orientation;
JVector position = rb.Position;
foreach (var ts in cShape.Shapes)
{
JVector pos = ts.Position;
JMatrix ori = ts.Orientation;
JVector.Transform(ref pos, ref orientation, out pos);
JVector.Add(ref pos, ref position, out pos);
JMatrix.Multiply(ref ori, ref orientation, out ori);
AddShapeToDrawList(ts.Shape, ori, pos);
}
}
}
public override void Iterate()
{
if (Rotate == Rotate.Clockwise)
{
Body1.IsActive = true;
JVector torque = new JVector(0, 0, 1);
torque = JVector.Transform(torque, Body1.Orientation);
torque = JVector.Transform(torque, Body1.Inertia);
Body1.AddTorque(torque * Body1.Mass);
}
if (Rotate == Rotate.Anticlockwise)
{
Body1.IsActive = true;
JVector torque = new JVector(0, 0, -1);
torque = JVector.Transform(torque, Body1.Orientation);
torque = JVector.Transform(torque, Body1.Inertia);
Body1.AddTorque(torque * Body1.Mass);
}
if (Direction == Walk.Forward)
{
Body1.IsActive = true;
JVector force = new JVector(1, 0, 0);
force = JVector.Transform(force, Body1.Orientation);
Body1.AddForce(force * Body1.Mass);
}
if (Direction == Walk.Backward)
{
Body1.IsActive = true;
JVector force = new JVector(-1, 0, 0);
force = JVector.Transform(force, Body1.Orientation);
Body1.AddForce(force * Body1.Mass);
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector p1, dp;
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Subtract(ref p1, ref anchor, out dp);
float deltaLength = dp.Length();
JVector n = anchor - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
effectiveMass = body1.inverseMass + JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
// CUSTOM: Modified to use the IsStatic property (plus the condition below).
if (!body1.IsStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
bool b1IsMulti = (body1.Shape is Multishape);
bool b2IsMulti = (body2.Shape is Multishape);
JVector point, normal;
float penetration;
if (!b1IsMulti && !b2IsMulti)
{
if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation,
ref body2.orientation, ref body1.position, ref body2.position,
out point, out normal, out penetration))
{
if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration))
{
JVector point1, point2;
FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2);
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
}
}
}
else if (b1IsMulti && b2IsMulti)
{
Multishape ms1 = (body1.Shape as Multishape);
Multishape ms2 = (body2.Shape as Multishape);
ms1 = ms1.RequestWorkingClone();
ms2 = ms2.RequestWorkingClone();
JBBox transformedBoundingBox = body2.boundingBox;
transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);
int ms1Length = ms1.Prepare(ref transformedBoundingBox);
transformedBoundingBox = body1.boundingBox;
transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);
int ms2Length = ms2.Prepare(ref transformedBoundingBox);
if (ms1Length == 0 || ms2Length == 0)
{
ms1.ReturnWorkingClone();
ms2.ReturnWorkingClone();
return;
}
for (int i = 0; i < ms1Length; i++)
{
ms1.SetCurrentShape(i);
for (int e = 0; e < ms2Length; e++)
{
ms2.SetCurrentShape(e);
if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
ref body2.orientation, ref body1.position, ref body2.position,
out point, out normal, out penetration))
{
if (this.RaisePassedNarrowphase(body1, body2, ref point, ref normal, penetration))
{
JVector point1, point2;
FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
}
}
}
}
ms1.ReturnWorkingClone();
ms2.ReturnWorkingClone();
}
else
{
RigidBody b1, b2;
if (body2.Shape is Multishape)
{
b1 = body2; b2 = body1;
}
else
{
b2 = body2; b1 = body1;
}
Multishape ms = (b1.Shape as Multishape);
ms = ms.RequestWorkingClone();
JBBox transformedBoundingBox = b2.boundingBox;
transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);
int msLength = ms.Prepare(ref transformedBoundingBox);
if (msLength == 0)
{
ms.ReturnWorkingClone();
return;
}
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
ref b2.orientation, ref b1.position, ref b2.position,
out point, out normal, out penetration))
{
if (this.RaisePassedNarrowphase(b1, b2, ref point, ref normal, penetration))
{
JVector point1, point2;
FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
}
}
}
ms.ReturnWorkingClone();
}
}
/// <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;
}
}
}
}
}
/// <summary>
/// Functionality which is applied before the physics is updated
/// </summary>
/// <param name="timestep"></param>
public override void PreStep(float timestep)
{
if (IsAnimated)
{
_oldRotation = Conversion.ToXnaQuaternion(JQuaternion.CreateFromMatrix(Orientation)).ToEuler().Y;
_newRotation = GameObject.transform.eulerAngles.Y;
// First calculate the correct orientation/rotation and then adjust the position with respect to the rotated COM
Orientation = JMatrix.CreateFromQuaternion(Conversion.ToJitterQuaternion(GameObject.transform.rotation));
Position = Conversion.ToJitterVector(GameObject.transform.position) + JVector.Transform(CenterOfMass, Orientation);
}
else
{
_oldRotation = GameObject.transform.eulerAngles.Y;
}
}
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
bool b1IsMulti = (body1.Shape is Multishape);
bool b2IsMulti = (body2.Shape is Multishape);
bool speculative = speculativeContacts ||
(body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts);
JVector point, normal;
float penetration;
if (!b1IsMulti && !b2IsMulti)
{
if (XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation,
ref body2.orientation, ref body1.position, ref body2.position,
out point, out normal, out penetration))
{
JVector point1, point2;
FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref point, ref normal, out point1, out point2);
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
}
else if (speculative)
{
JVector hit1, hit2;
if (GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation,
ref body1.position, ref body2.position, out hit1, out hit2, out normal))
{
JVector delta = hit2 - hit1;
if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
{
penetration = delta * normal;
if (penetration < 0.0f)
{
RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
}
}
}
}
}
else if (b1IsMulti && b2IsMulti)
{
Multishape ms1 = (body1.Shape as Multishape);
Multishape ms2 = (body2.Shape as Multishape);
ms1 = ms1.RequestWorkingClone();
ms2 = ms2.RequestWorkingClone();
JBBox transformedBoundingBox = body2.boundingBox;
transformedBoundingBox.InverseTransform(ref body1.position, ref body1.orientation);
int ms1Length = ms1.Prepare(ref transformedBoundingBox);
transformedBoundingBox = body1.boundingBox;
transformedBoundingBox.InverseTransform(ref body2.position, ref body2.orientation);
int ms2Length = ms2.Prepare(ref transformedBoundingBox);
if (ms1Length == 0 || ms2Length == 0)
{
ms1.ReturnWorkingClone();
ms2.ReturnWorkingClone();
return;
}
for (int i = 0; i < ms1Length; i++)
{
ms1.SetCurrentShape(i);
for (int e = 0; e < ms2Length; e++)
{
ms2.SetCurrentShape(e);
if (XenoCollide.Detect(ms1, ms2, ref body1.orientation,
ref body2.orientation, ref body1.position, ref body2.position,
out point, out normal, out penetration))
{
JVector point1, point2;
FindSupportPoints(body1, body2, ms1, ms2, ref point, ref normal, out point1, out point2);
RaiseCollisionDetected(body1, body2, ref point1, ref point2, ref normal, penetration);
}
else if (speculative)
{
JVector hit1, hit2;
if (GJKCollide.ClosestPoints(ms1, ms2, ref body1.orientation, ref body2.orientation,
ref body1.position, ref body2.position, out hit1, out hit2, out normal))
{
JVector delta = hit2 - hit1;
if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
{
penetration = delta * normal;
if (penetration < 0.0f)
{
RaiseCollisionDetected(body1, body2, ref hit1, ref hit2, ref normal, penetration);
}
}
}
}
}
}
ms1.ReturnWorkingClone();
ms2.ReturnWorkingClone();
}
else
{
RigidBody b1, b2;
if (body2.Shape is Multishape)
{
b1 = body2; b2 = body1;
}
else
{
b2 = body2; b1 = body1;
}
Multishape ms = (b1.Shape as Multishape);
ms = ms.RequestWorkingClone();
JBBox transformedBoundingBox = b2.boundingBox;
transformedBoundingBox.InverseTransform(ref b1.position, ref b1.orientation);
int msLength = ms.Prepare(ref transformedBoundingBox);
if (msLength == 0)
{
ms.ReturnWorkingClone();
return;
}
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
ref b2.orientation, ref b1.position, ref b2.position,
out point, out normal, out penetration))
{
JVector point1, point2;
FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out point1, out point2);
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, penetration);
}
else if (speculative)
{
JVector hit1, hit2;
if (GJKCollide.ClosestPoints(ms, b2.Shape, ref b1.orientation, ref b2.orientation,
ref b1.position, ref b2.position, out hit1, out hit2, out normal))
{
JVector delta = hit2 - hit1;
if (delta.LengthSquared() < (body1.sweptDirection - body2.sweptDirection).LengthSquared())
{
penetration = delta * normal;
if (penetration < 0.0f)
{
RaiseCollisionDetected(b1, b2, ref hit1, ref hit2, ref normal, penetration);
}
}
}
}
}
ms.ReturnWorkingClone();
}
}
// this should only be called if this shape is colliding
public override void UpdateAxes(float orientation)
{
this.xAxis = JVector.Transform(JVector.Left, JMatrix.CreateRotationZ(orientation));
this.yAxis = JVector.Transform(JVector.Up, JMatrix.CreateRotationZ(orientation));
}
public static void ExtractData(List <JVector> vertices, List <TriangleVertexIndices> indices, Model model)
{
Matrix[] bones_ = new Matrix[model.Bones.Count];
model.CopyAbsoluteBoneTransformsTo(bones_);
foreach (ModelMesh modelmesh in model.Meshes)
{
JMatrix xform = Helper.ToJitterMatrix(bones_[modelmesh.ParentBone.Index]);
foreach (ModelMeshPart meshPart in modelmesh.MeshParts)
{
// Before we add any more where are we starting from
int offset = vertices.Count;
// Read the format of the vertex buffer
VertexDeclaration declaration = meshPart.VertexBuffer.VertexDeclaration;
VertexElement[] vertexElements = declaration.GetVertexElements();
// Find the element that holds the position
VertexElement vertexPosition = new VertexElement();
foreach (VertexElement vert in vertexElements)
{
if (vert.VertexElementUsage == VertexElementUsage.Position &&
vert.VertexElementFormat == VertexElementFormat.Vector3)
{
vertexPosition = vert;
// There should only be one
break;
}
}
// Check the position element found is valid
if (vertexPosition == null ||
vertexPosition.VertexElementUsage != VertexElementUsage.Position ||
vertexPosition.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new Exception("Model uses unsupported vertex format!");
}
// This where we store the vertices until transformed
JVector[] allVertex = new JVector[meshPart.NumVertices];
// Read the vertices from the buffer in to the array
meshPart.VertexBuffer.GetData <JVector>(
meshPart.VertexOffset * declaration.VertexStride + vertexPosition.Offset,
allVertex,
0,
meshPart.NumVertices,
declaration.VertexStride);
// Transform them based on the relative bone location and the world if provided
for (int i = 0; i != allVertex.Length; ++i)
{
JVector.Transform(ref allVertex[i], ref xform, out allVertex[i]);
}
// Store the transformed vertices with those from all the other meshes in this model
vertices.AddRange(allVertex);
// Find out which vertices make up which triangles
if (meshPart.IndexBuffer.IndexElementSize != IndexElementSize.SixteenBits)
{
// This could probably be handled by using int in place of short but is unnecessary
throw new Exception("Model uses 32-bit indices, which are not supported.");
}
// Each primitive is a triangle
short[] indexElements = new short[meshPart.PrimitiveCount * 3];
meshPart.IndexBuffer.GetData <short>(
meshPart.StartIndex * 2,
indexElements,
0,
meshPart.PrimitiveCount * 3);
// Each TriangleVertexIndices holds the three indexes to each vertex that makes up a triangle
TriangleVertexIndices[] tvi = new TriangleVertexIndices[meshPart.PrimitiveCount];
for (int i = 0; i != tvi.Length; ++i)
{
// The offset is because we are storing them all in the one array and the
// vertices were added to the end of the array.
tvi[i].I0 = indexElements[i * 3 + 0] + offset;
tvi[i].I1 = indexElements[i * 3 + 1] + offset;
tvi[i].I2 = indexElements[i * 3 + 2] + offset;
}
// Store our triangles
indices.AddRange(tvi);
}
}
}
// TODO: Apply edge forgiveness when sliding off the edge of a triangle (and becoming airborne).
// TODO: For pseudo-static wall control, probably need to rotate the normal based on body orientation each step.
public override void PreStep(float step)
{
// The alternative here is body-controlled (i.e. wall control on a pseudo-static body).
bool isMeshControlled = wall != null;
var body = Parent.ControllingBody;
var v = isMeshControlled ? body.LinearVelocity.ToVec3() : Parent.ManualVelocity;
// TODO: Apply jump deceleration as appropriate (feels weird to keep gaining height when jump is released).
// "Wall gravity" only applies when moving downward (in order to give the player a little more control when
// setting up wall jumps).
v.y -= (v.y > 0 ? PhysicsConstants.Gravity : wallGravity) * step;
// TODO: Quickly decelerate if the wall is hit at a downward speed faster than terminal.
if (v.y < -wallTerminalSpeed)
{
v.y = -wallTerminalSpeed;
}
// TODO: Consider applying wall press logic (i.e. only move side to side if you're angled enough).
var flatV = v.swizzle.xz;
// Acceleration
if (Utilities.LengthSquared(FlatDirection) > 0)
{
var perpendicular = flatNormal.swizzle.xz;
perpendicular = new vec2(-perpendicular.y, perpendicular.x);
flatV += Utilities.Project(FlatDirection, perpendicular) * acceleration * step;
// TODO: Quickly decelerate local max if the wall is hit with fast sideways speed.
// This limits maximum speed based on flat direction. To me, this feels more natural than accelerating
// up to full speed even when barely moving sideways (relative to the wall).
var localMax = Math.Abs(Utilities.Dot(FlatDirection, perpendicular)) * maxSpeed;
if (Utilities.LengthSquared(flatV) > localMax * localMax)
{
flatV = Utilities.Normalize(flatV) * localMax;
}
}
// Deceleration
else if (Utilities.LengthSquared(flatV) > 0)
{
int oldSign = Math.Sign(flatV.x != 0 ? flatV.x : flatV.y);
flatV -= Utilities.Normalize(flatV) * deceleration * step;
int newSign = Math.Sign(flatV.x != 0 ? flatV.x : flatV.y);
if (oldSign != newSign)
{
flatV = vec2.Zero;
}
}
v.x = flatV.x;
v.z = flatV.y;
if (isMeshControlled)
{
body.LinearVelocity = v.ToJVector();
}
else
{
Parent.ManualVelocity = v;
Parent.ManualPosition += JVector.Transform(v.ToJVector(), JMatrix.Inverse(wallBody.Orientation)) *
step;
}
// TODO: Apply a thin forgiveness range for staying on a wall.
var d = Utilities.Dot(FlatDirection, flatNormal.swizzle.xz);
// This means that the flat direction is pressing away the wall. A thin forgiveness angle (specified as a
// dot product value) is used to help stick the player while still moving in a direction *near* parallel
// to the wall.
if (d > stickForgiveness)
{
wallStickTimer.IsPaused = false;
}
}
public override void DebugDraw(IDebugDrawer drawer)
{
drawer.DrawLine(body1.position + r1,
body1.position + r1 + JVector.Transform(lineNormal, body1.orientation) * 100.0f);
}
/// <summary>
/// Gets the position of the wheel in world space.
/// </summary>
/// <returns>The position of the wheel in world space.</returns>
public JVector GetWorldPosition()
{
return(car.Position +
JVector.Transform(Position + JVector.Up * displacement, car.Orientation));
}
/// <summary>
/// Raycasts a single body. NOTE: For performance reasons terrain and trianglemeshshape aren't checked
/// against rays (rays are of infinite length). They are checked against segments
/// which start at rayOrigin and end in rayOrigin + rayDirection.
/// </summary>
// CUSTOM: Added triangle as an output parameter.
// TODO: Can this be optimized using this? https://en.m.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
#region public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction)
public override bool Raycast(RigidBody body, JVector rayOrigin, JVector rayDirection, out JVector normal, out float fraction, out JVector[] triangle)
{
fraction = float.MaxValue;
normal = JVector.Zero;
triangle = null;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape multishape)
{
Multishape ms = multishape.RequestWorkingClone();
bool multiShapeCollides = false;
JVector.Subtract(ref rayOrigin, ref body.position, out var transformedOrigin);
JVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
JVector.Transform(ref rayDirection, ref body.invOrientation, out var transformedDirection);
int msLength = ms.Prepare(ref transformedOrigin, ref transformedDirection);
for (int i = 0; i < msLength; i++)
{
ms.SetCurrentShape(i);
if (GJKCollide.Raycast(ms, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out var tempFraction, out var tempNormal))
{
if (tempFraction < fraction)
{
if (useTerrainNormal && ms is TerrainShape terrainShape)
{
terrainShape.CollisionNormal(out tempNormal);
JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape tMesh)
{
triangle = tMesh.CurrentTriangle;
tMesh.CollisionNormal(out tempNormal);
JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
}
ms.ReturnWorkingClone();
return(multiShapeCollides);
}
return(GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out fraction, out normal));
}
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);
}
}
/// <summary>
/// Initializes a contact.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="point1">The collision point in worldspace</param>
/// <param name="point2">The collision point in worldspace</param>
/// <param name="n">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public void Initialize(RigidBody body1, RigidBody body2, ref JVector point1, ref JVector point2, ref JVector n,
float penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1; this.body2 = body2;
this.normal = n; normal.Normalize();
this.p1 = point1; this.p2 = point2;
this.newContact = newContact;
JVector.Subtract(ref p1, ref body1.position, out relativePos1);
JVector.Subtract(ref p2, ref body2.position, out relativePos2);
JVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
JVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
considerAngularVelocity = true;
if (settings.considerAngularVelocityCallback != null)
{
considerAngularVelocity = settings.considerAngularVelocityCallback(body1, body2);
}
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
accumulatedNormalImpulse = 0.0f;
accumulatedTangentImpulse = 0.0f;
lostSpeculativeBounce = 0.0f;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = JMath.Max(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = JMath.Min(body1.material.staticFriction, body2.material.staticFriction);
dynamicFriction = JMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
restitution = JMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / 2.0f;
dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / 2.0f;
restitution = (body1.material.restitution + body2.material.restitution) / 2.0f;
break;
case ContactSettings.MaterialCoefficientMixingType.UseCallback:
staticFriction = settings.calculateStaticFrictionCallback(body1, body2);
dynamicFriction = settings.calculateDynamicFrictionCallback(body1, body2);
restitution = settings.calculateRestitutionCallback(body1, body2);
break;
}
}
this.settings = settings;
}
protected override void Update(GameTime gameTime)
{
padState = GamePad.GetState(PlayerIndex.One);
keyState = Keyboard.GetState();
mouseState = Mouse.GetState();
if (PressedOnce(Keys.Escape, Buttons.Back))
{
Exit();
}
if (PressedOnce(Keys.M, Buttons.A))
{
multithread = !multithread;
}
if (PressedOnce(Keys.P, Buttons.A))
{
var e = World.RigidBodies.GetEnumerator();
e.MoveNext(); e.MoveNext(); e.MoveNext();
e.MoveNext(); e.MoveNext(); e.MoveNext();
e.MoveNext(); e.MoveNext(); e.MoveNext();
(e.Current as RigidBody).IsStatic = true;
e.MoveNext();
(e.Current as RigidBody).IsStatic = true;
}
if ((mouseState.LeftButton == ButtonState.Pressed &&
mousePreviousState.LeftButton == ButtonState.Released) ||
(padState.IsButtonDown(Buttons.RightThumbstickDown) &&
gamePadPreviousState.IsButtonUp(Buttons.RightThumbstickUp)))
{
var ray = Conversion.ToJitterVector(RayTo(mouseState.X, mouseState.Y));
var camp = Conversion.ToJitterVector(Camera.Position);
ray = JVector.Normalize(ray) * 100;
bool result = World.CollisionSystem.Raycast(camp, ray, RaycastCallback, out grabBody, out hitNormal, out float fraction);
if (result)
{
hitPoint = camp + (fraction * ray);
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
var lanchor = hitPoint - grabBody.Position;
lanchor = JVector.Transform(lanchor, JMatrix.Transpose(grabBody.Orientation));
grabConstraint = new SingleBodyConstraints.PointOnPoint(grabBody, lanchor)
{
Softness = 0.01f,
BiasFactor = 0.1f
};
World.AddConstraint(grabConstraint);
hitDistance = (Conversion.ToXNAVector(hitPoint) - Camera.Position).Length();
scrollWheel = mouseState.ScrollWheelValue;
grabConstraint.Anchor = hitPoint;
}
}
if (mouseState.LeftButton == ButtonState.Pressed || padState.IsButtonDown(Buttons.RightThumbstickDown))
{
hitDistance += (mouseState.ScrollWheelValue - scrollWheel) * 0.01f;
scrollWheel = mouseState.ScrollWheelValue;
if (grabBody != null)
{
var ray = RayTo(mouseState.X, mouseState.Y); ray.Normalize();
grabConstraint.Anchor = Conversion.ToJitterVector(Camera.Position + (ray * hitDistance));
grabBody.IsActive = true;
if (!grabBody.IsStatic)
{
grabBody.LinearVelocity *= 0.98f;
grabBody.AngularVelocity *= 0.98f;
}
}
}
else
{
if (grabConstraint != null)
{
World.RemoveConstraint(grabConstraint);
}
grabBody = null;
grabConstraint = null;
}
if (PressedOnce(Keys.Space, Buttons.B))
{
SpawnRandomPrimitive(Conversion.ToJitterVector(Camera.Position),
Conversion.ToJitterVector((Camera.Target - Camera.Position) * 40.0f));
}
if (PressedOnce(Keys.Add, Buttons.X))
{
DestroyCurrentScene();
currentScene++;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
if (PressedOnce(Keys.Subtract, Buttons.Y))
{
DestroyCurrentScene();
currentScene += PhysicScenes.Count - 1;
currentScene = currentScene % PhysicScenes.Count;
PhysicScenes[currentScene].Build();
}
UpdateDisplayText(gameTime);
float step = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (step > 1.0f / 100.0f)
{
step = 1.0f / 100.0f;
}
World.Step(step, multithread);
gamePadPreviousState = padState;
keyboardPreviousState = keyState;
mousePreviousState = mouseState;
base.Update(gameTime);
}