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
};
}
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 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;
}