/// <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>
#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)
{
fraction = float.MaxValue; normal = JVector.Zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
JVector tempNormal; float tempFraction;
bool multiShapeCollides = false;
JVector transformedOrigin; JVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
JVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
JVector transformedDirection; JVector.Transform(ref rayDirection, ref body.invOrientation, out 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 tempFraction, out tempNormal))
{
if (tempFraction < fraction)
{
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out tempNormal);
JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
JVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
normal = tempNormal;
fraction = tempFraction;
multiShapeCollides = true;
}
}
}
ms.ReturnWorkingClone();
return(multiShapeCollides);
}
else
{
return(GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position,
ref rayOrigin, ref rayDirection, out fraction, out normal));
}
}
/// <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>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public Contact AddContact(JVector point1, JVector point2, JVector normal, float penetration,
ContactSettings contactSettings)
{
JVector relPos1;
JVector.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 2)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = poolContact.GetNew();
contact.Prepare(poolContact);
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(JVector force, JVector pos)
{
JVector.Add(ref this.force, ref force, out this.force);
JVector.Subtract(ref pos, ref this.position, out pos);
JVector.Cross(ref pos, ref force, out pos);
JVector.Add(ref pos, ref this.torque, out this.torque);
}
/// <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);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
}
public void GetNormal(out JVector normal)
{
JVector sum;
JVector.Subtract(ref owner.points[indices.I1].position, ref owner.points[indices.I0].position, out sum);
JVector.Subtract(ref owner.points[indices.I2].position, ref owner.points[indices.I0].position, out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
public PointOnPoint(RigidBody body1, RigidBody body2, JVector anchor) : base(body1, body2)
{
JVector.Subtract(anchor, body1.position, out localAnchor1);
JVector.Subtract(anchor, body2.position, out localAnchor2);
JVector.Transform(localAnchor1, body1.invOrientation, out localAnchor1);
JVector.Transform(localAnchor2, body2.invOrientation, out localAnchor2);
}
/// <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);
JMatrix o1 = JMatrix.CreateRotationZ(body1.invOrientation);
JMatrix o2 = JMatrix.CreateRotationZ(body2.invOrientation);
JVector.Transform(ref relativePos1, ref o1, out realRelPos1);
JVector.Transform(ref relativePos2, ref o2, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// 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;
}
}
this.settings = settings;
}
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public PointOnPoint(RigidBody body1, RigidBody body2, JVector anchor)
: base(body1, body2)
{
JVector.Subtract(ref anchor, ref body1.position, out localAnchor1);//jmatrix di ancor-jmatrix di position
JVector.Subtract(ref anchor, ref body2.position, out localAnchor2);
JVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);//trasforma una matrice in un vettore
JVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
}
public PointOnLine(RigidBody body1, RigidBody body2, JVector lineStartPointBody1, JVector pointBody2) : base(body1, body2)
{
JVector.Subtract(lineStartPointBody1, body1.position, out localAnchor1);
JVector.Subtract(pointBody2, body2.position, out localAnchor2);
JVector.Transform(localAnchor1, body1.invOrientation, out localAnchor1);
JVector.Transform(localAnchor2, body2.invOrientation, out localAnchor2);
lineNormal = JVector.Normalize(lineStartPointBody1 - pointBody2);
}
public PointPointDistance(RigidBody body1, RigidBody body2, JVector anchor1, JVector anchor2) : base(body1, body2)
{
JVector.Subtract(anchor1, body1.position, out localAnchor1);
JVector.Subtract(anchor2, body2.position, out localAnchor2);
JVector.Transform(localAnchor1, body1.invOrientation, out localAnchor1);
JVector.Transform(localAnchor2, body2.invOrientation, out localAnchor2);
Distance = (anchor1 - anchor2).Length();
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(double timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
JVector l = JVector.Transform(lineNormal, body1.orientation);
l.Normalize();
JVector t = (p1 - p2) % l;
if (t.LengthSquared() != 0.0f)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -1.0f * t; // linearVel Body2
jacobian[3] = -1.0f * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p2 - p1)).Length() * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
// sort cached points so most isolated points come first
private int SortCachedPoints(ref JVector realRelPos1, float pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
int maxPenetrationIndex = -1;
float maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res0 = cross.LengthSquared();
}
if (maxPenetrationIndex != 1)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res1 = cross.LengthSquared();
}
if (maxPenetrationIndex != 2)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res2 = cross.LengthSquared();
}
if (maxPenetrationIndex != 3)
{
JVector a0; JVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
JVector b0; JVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
JVector cross; JVector.Cross(ref a0, ref b0, out cross);
res3 = cross.LengthSquared();
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
/// <summary>
/// Initializes a new instance of the DistanceConstraint class.
/// </summary>
/// <param name="body1">The first body.</param>
/// <param name="body2">The second body.</param>
/// <param name="anchor1">The anchor point of the first body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
/// <param name="anchor2">The anchor point of the second body in world space.
/// The distance is given by the initial distance between both anchor points.</param>
public PointPointDistance(RigidBody body1, RigidBody body2, JVector anchor1, JVector anchor2)
: base(body1, body2)
{
JVector.Subtract(ref anchor1, ref body1.position, out localAnchor1);
JVector.Subtract(ref anchor2, ref body2.position, out localAnchor2);
JVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
JVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
distance = (anchor1 - anchor2).Length();//per calcolare la distanza calcola la lunghezza della diagonale
}
public override void SupportMapping(ref JVector direction, out JVector result)
{
JVector temp1, temp2 = JVector.Zero;
for (int i = 0; i < shapes.Count; i++)
{
shapes[i].SupportMapping(ref direction, out temp1);
JVector.Add(ref temp1, ref temp2, out temp2);
}
JVector.Subtract(ref temp2, ref shifted, out result);
}
public static JVector ComputeNormal(JVector p0, JVector p1, JVector p2, WindingTypes winding,
bool shouldNormalize = true)
{
var v0 = JVector.Subtract(p1, p0);
var v1 = JVector.Subtract(p2, p0);
// This calculation is the same as the one used in a constructor below, but due to using JVector vs. vec3,
// it's easier to just duplicate the code.
var v = JVector.Cross(v0, v1) * (winding == WindingTypes.Clockwise ? 1 : -1);
return(shouldNormalize ? JVector.Normalize(v) : v);
}
public override void PrepareForIteration(float timestep)
{
JVector.Transform(localAnchor1, body1.orientation, out r1);
JVector.Transform(localAnchor2, body2.orientation, out r2);
JVector.Add(body1.position, r1, out var p1);
JVector.Add(body2.position, r2, out var p2);
JVector.Subtract(p2, p1, out _);
var l = JVector.Transform(lineNormal, body1.orientation);
l = JVector.Normalize(l);
var t = (p1 - p2) % l;
if (t.LengthSquared() != 0.0f)
{
t = JVector.Normalize(t);
}
t %= l;
jacobian[0] = t;
jacobian[1] = (r1 + p2 - p1) % t;
jacobian[2] = -1.0f * t;
jacobian[3] = -1.0f * r2 % t;
effectiveMass = body1.inverseMass + body2.inverseMass
+ (JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1])
+ (JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]);
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = 1.0f / effectiveMass;
}
bias = -(l % (p2 - p1)).Length() * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <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 HingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis) : base(world)
{
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);
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(JVector force, JVector pos)
{
JVector.Add(ref this.force, ref force, out this.force);
JVector.Subtract(ref pos, ref this.position, out pos);
// 3d version
//JVector.Cross(ref pos, ref force, out pos);
//JVector.Add(ref pos, ref this.torque, out this.torque);
//body->t += cpvcross(r, force);
this.torque += JVector.Cross(pos, force);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector dp;
JVector.Subtract(ref body2.position, ref body1.position, out dp);
float deltaLength = dp.Length() - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
JVector n = dp;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
//jacobian[1] = -1.0f * (r1 % n);
jacobian[1] = 1.0f * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1];
}
}
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(float timestep)
{
JVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
JVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
JVector p1, p2, dp;
JVector.Add(ref body1.position, ref r1, out p1);
JVector.Add(ref body2.position, ref r2, out p2);
JVector.Subtract(ref p2, ref p1, out dp);
float deltaLength = dp.Length();
JVector n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n.Normalize();
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * biasFactor * (1.0f / timestep);
// 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);
}
if (!body2.IsStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
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
};
}
/// <summary>
/// Calculates the inertia of a box with the sides of the multishape.
/// </summary>
public override void CalculateMassInertia()
{
geomCen = JVector.Zero;
// TODO: calc this right
inertia = JMatrix.Identity;
JVector size; JVector.Subtract(ref boundingBox.Max, ref boundingBox.Min, out size);
mass = size.X * size.Y * size.Z;
inertia.M11 = (1.0f / 12.0f) * mass * (size.Y * size.Y + size.Z * size.Z);
inertia.M22 = (1.0f / 12.0f) * mass * (size.X * size.X + size.Z * size.Z);
inertia.M33 = (1.0f / 12.0f) * mass * (size.X * size.X + size.Y * size.Y);
}
public override void PrepareForIteration(float timestep)
{
JVector.Subtract(body2.position, body1.position, out var dp);
float deltaLength = dp.Length() - Distance;
if (Behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= 0.0f)
{
skipConstraint = true;
}
else if (Behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= 0.0f)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
var n = dp;
if (n.LengthSquared() != 0.0f)
{
n = JVector.Normalize(n);
}
jacobian[0] = -1.0f * n;
jacobian[1] = 1.0f * n;
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[1];
}
}
}
public override void PrepareForIteration(float timestep)
{
JVector.Transform(localAnchor1, body1.orientation, out r1);
JVector.Transform(localAnchor2, body2.orientation, out r2);
JVector.Add(body1.position, r1, out var p1);
JVector.Add(body2.position, r2, out var p2);
JVector.Subtract(p2, p1, out var dp);
float deltaLength = dp.Length();
var n = p2 - p1;
if (n.LengthSquared() != 0.0f)
{
n = JVector.Normalize(n);
}
jacobian[0] = -1.0f * n;
jacobian[1] = -1.0f * (r1 % n);
jacobian[2] = 1.0f * n;
jacobian[3] = r2 % n;
effectiveMass = body1.inverseMass + body2.inverseMass
+ (JVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1])
+ (JVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3]);
softnessOverDt = Softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = 1.0f / effectiveMass;
bias = deltaLength * BiasFactor * (1.0f / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * AppliedImpulse * jacobian[0];
body1.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * AppliedImpulse * jacobian[2];
body2.angularVelocity += JVector.Transform(AppliedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref JBBox aabb, EChild child, out JBBox result)
{
JVector dims;
JVector.Subtract(ref aabb.Max, ref aabb.Min, out dims);
JVector.Multiply(ref dims, 0.5f, out dims);
JVector offset = JVector.Zero;
switch (child)
{
case EChild.PPP: offset = new JVector(1, 1, 1); break;
case EChild.PPM: offset = new JVector(1, 1, 0); break;
case EChild.PMP: offset = new JVector(1, 0, 1); break;
case EChild.PMM: offset = new JVector(1, 0, 0); break;
case EChild.MPP: offset = new JVector(0, 1, 1); break;
case EChild.MPM: offset = new JVector(0, 1, 0); break;
case EChild.MMP: offset = new JVector(0, 0, 1); break;
case EChild.MMM: offset = new JVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new JBBox();
result.Min = new JVector(offset.X * dims.X, offset.Y * dims.Y, offset.Z * dims.Z);
JVector.Add(ref result.Min, ref aabb.Min, out result.Min);
JVector.Add(ref result.Min, ref dims, out result.Max);
// expand it just a tiny bit just to be safe!
double extra = 0.00001f;
JVector temp; JVector.Multiply(ref dims, extra, out temp);
JVector.Subtract(ref result.Min, ref temp, out result.Min);
JVector.Add(ref result.Max, ref temp, out result.Max);
}
private int GetCacheEntry(ref JVector realRelPos1, float contactBreakThreshold)
{
float shortestDist = contactBreakThreshold * contactBreakThreshold;
int size = contactList.Count;
int nearestPoint = -1;
for (int i = 0; i < size; i++)
{
JVector diffA; JVector.Subtract(ref contactList[i].relativePos1, ref realRelPos1, out diffA);
float distToManiPoint = diffA.LengthSquared();
if (distToManiPoint < shortestDist)
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return(nearestPoint);
}
/// <summary>
/// Sets the current shape. First <see cref="Prepare"/> has to be called.
/// After SetCurrentShape the shape immitates another shape.
/// </summary>
/// <param name="index"></param>
public override void SetCurrentShape(int index)
{
vecs[0] = octree.GetVertex(octree.tris[potentialTriangles[index]].I0);
vecs[1] = octree.GetVertex(octree.tris[potentialTriangles[index]].I1);
vecs[2] = octree.GetVertex(octree.tris[potentialTriangles[index]].I2);
JVector sum = vecs[0];
JVector.Add(ref sum, ref vecs[1], out sum);
JVector.Add(ref sum, ref vecs[2], out sum);
JVector.Multiply(ref sum, 1.0f / 3.0f, out sum);
geomCen = sum;
JVector.Subtract(ref vecs[1], ref vecs[0], out sum);
JVector.Subtract(ref vecs[2], ref vecs[0], out normal);
JVector.Cross(ref sum, ref normal, out normal);
}
public static int FindNearestTrianglePoint(SoftBody sb, int id, ref JVector point)
{
SoftBody.Triangle triangle = sb.dynamicTree.GetUserData(id);
JVector p;
p = sb.points[triangle.indices.I0].position;
JVector.Subtract(ref p, ref point, out p);
float length0 = p.LengthSquared();
p = sb.points[triangle.indices.I1].position;
JVector.Subtract(ref p, ref point, out p);
float length1 = p.LengthSquared();
p = sb.points[triangle.indices.I2].position;
JVector.Subtract(ref p, ref point, out p);
float length2 = p.LengthSquared();
if (length0 < length1)
{
if (length0 < length2)
{
return(triangle.indices.I0);
}
else
{
return(triangle.indices.I2);
}
}
else
{
if (length1 < length2)
{
return(triangle.indices.I1);
}
else
{
return(triangle.indices.I2);
}
}
}
/// <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);
}
}