public override void Iterate()
{
deltaVelocity = TargetVelocity - Body1.LinearVelocity;
deltaVelocity.Y = 0.0f;
// determine how 'stiff' the character follows the target velocity
deltaVelocity *= this.Stiffness;
if (deltaVelocity.LengthSquared() != 0.0f)
{
// activate it, in case it fall asleep :)
Body1.IsActive = true;
Body1.ApplyImpulse(deltaVelocity * Body1.Mass);
}
if (shouldIJump)
{
Body1.IsActive = true;
Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);
if (!BodyWalkingOn.IsStatic)
{
BodyWalkingOn.IsActive = true;
// apply the negative impulse to the other body
BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass);
}
}
}
public override void Iterate()
{
deltaVelocity = TargetVelocity - Body1.LinearVelocity;
deltaVelocity.Y = 0.0f;
deltaVelocity *= 0.02f;
if (BodyWalkingOn == null)
{
deltaVelocity *= .1f;
}
if (deltaVelocity.LengthSquared() != 0.0f)
{
// activate it, in case it fall asleep :)
Body1.IsActive = true;
Body1.ApplyImpulse(deltaVelocity * Body1.Mass);
}
if (shouldIJump)
{
Body1.IsActive = true;
Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);
if (!BodyWalkingOn.IsStatic)
{
BodyWalkingOn.IsActive = true;
// apply the negative impulse to the other body
BodyWalkingOn.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass, contactPoint);
}
}
}
/// <summary>
/// Get the position so that the object does not collide with the object anymore.
/// Important: this is used raycasting checks, as this needs some more corner-cases.
/// </summary>
/// <param name="collider">The collider which is the player</param>
/// <param name="collisionPoint">Point of the collision in the scene</param>
/// <param name="hitNormal">Collision-normal of the object with which the player collided</param>
/// <returns>Position which is possible for the collider so there is no collision.</returns>
private JVector GetPosition(Collider collider, JVector collisionPoint, JVector hitNormal)
{
JVector bbSize = collider.BoundingBoxSize;
GameObject gameObject = collider.GameObject;
// Calculate the size of the forward and right vector based on the bounding-box.
JVector forward = 0.5f * bbSize.Z * Conversion.ToJitterVector(gameObject.transform.Forward);
JVector right = 0.5f * bbSize.X * Conversion.ToJitterVector(gameObject.transform.Right);
JVector d = Conversion.ToJitterVector(gameObject.transform.Forward);
if (hitNormal.IsZero())
{
return(bbSize.Z * -0.5f * Conversion.ToJitterVector(gameObject.transform.Forward));
}
// Correct directions
TurnIntoDirectionOf(ref forward, hitNormal);
TurnIntoDirectionOf(ref right, hitNormal);
TurnIntoDirectionOf(ref d, hitNormal);
// Calculate the projected length (half-length) of the player on the hit-normal.
JVector lengthOnNormal = ProjectOn(forward, hitNormal);
JVector widthOnNormal = ProjectOn(right, hitNormal);
JVector projectedSize = lengthOnNormal + widthOnNormal;
JVector margin = ProjectOn(projectedSize, d);
JVector endPosition = collisionPoint + margin;
// Check if it hit a corner and it could actually go closer
JVector plane = new JVector(-hitNormal.Z, 0, hitNormal.X);
TurnIntoDirectionOf(ref plane, -1 * forward);
plane.Normalize();
// Check if there is an intersection between the front-"plane" of the player and the collided object
JVector intersection;
JVector frontMiddle = endPosition + forward;
JVector frontRight = endPosition + forward - right;
JVector planeEnd = collisionPoint + MathHelper.Max(bbSize.X, bbSize.Z) * plane;
if (DoIntersect(frontMiddle, frontRight, collisionPoint, planeEnd, out intersection))
{
intersection.Y = collider.Position.Y;
JVector colToIntersection = intersection - collisionPoint;
JVector margin2 = ProjectOn(colToIntersection, d);
margin = margin.LengthSquared() > margin2.LengthSquared() ? margin2 : margin;
endPosition = collisionPoint + margin;
}
//// Todo: Visual debuggin might be removed in the end
//PhysicsDrawer.Instance.ClearPointsToDraw();
//PhysicsDrawer.Instance.AddPointToDraw(Conversion.ToXnaVector(collisionPoint));
//PhysicsDrawer.Instance.AddPointToDraw(Conversion.ToXnaVector(endPosition));
//return collider.Position;
return(endPosition);
}
/// <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 override void Iterate()
{
_deltaVelocity = TargetVelocity - Body1.LinearVelocity;
_deltaVelocity.Y = 0.0f;
_deltaVelocity *= Stiffness;
if (Math.Abs(_deltaVelocity.LengthSquared()) > 0.00001f)
{
Body1.IsActive = true;
Body1.ApplyImpulse(_deltaVelocity * Body1.Mass);
}
if (_shouldIJump)
{
Body1.IsActive = true;
Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);
if (!BodyWalkingOn.IsStatic)
{
BodyWalkingOn.IsActive = true;
BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass);
}
}
}
/// <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);
}
}
public PointOnLine(RigidBody body, JVector localAnchor, JVector lineDirection) : base(body, null)
{
if (lineDirection.LengthSquared() == 0.0f)
{
throw new ArgumentException("Line direction can't be zero", nameof(lineDirection));
}
localAnchor1 = localAnchor;
anchor = body.position + JVector.Transform(localAnchor, body.orientation);
lineNormal = lineDirection;
lineNormal = JVector.Normalize(lineNormal);
}
/// <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);
}
}
/// <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);
}
}
public override void Iterate()
{
deltaVelocity = TargetVelocity - Body1.LinearVelocity;
if (deltaVelocity.Y > 0 || isJumping)
{
deltaVelocity.Y = 0;
}
// determine how 'stiff' the character follows the target velocity
deltaVelocity *= Stiff;
if (deltaVelocity.LengthSquared() != 0.0f)
{
// activate it, in case it fall asleep :)
Body1.IsActive = true;
Body1.ApplyImpulse(deltaVelocity * Body1.Mass);
}
if (shouldIJump)
{
isJumping = true;
Body1.IsActive = true;
Body1.ApplyImpulse(JumpVelocity * JVector.Up * Body1.Mass);
System.Diagnostics.Debug.WriteLine("JUMP! " + DateTime.Now.Second.ToString());
if (!BodyWalkingOn.IsStatic && World.RigidBodies.Contains(BodyWalkingOn))
{
BodyWalkingOn.IsActive = true;
// apply the negative impulse to the other body
BodyWalkingOn.ApplyImpulse(-1.0f * JumpVelocity * JVector.Up * Body1.Mass);
}
}
//if (!isJumping && !isGrounded)
// deltaVelocity.Y = -1.0f;
//Text = isJumping + " " + shouldIJump;
}
/// <summary>
/// Detect a possible collision between the object and a planned end-position.
/// </summary>
/// <param name="rigidBody">Rigid-body to test for</param>
/// <param name="start">Start of the test-ray</param>
/// <param name="end">End of the test-ray</param>
/// <returns>Position on the ray which is still possible to place the rigid-body without collision</returns>
public JVector DetectCollision(RigidBody rigidBody, JVector start, JVector end)
{
JVector p = rigidBody.Position;
JVector d = end - start;
JVector d5 = 5 * d;
Collider collider = rigidBody as Collider;
RigidBody resBody;
JVector hitNormal;
float fraction;
bool result = World.CollisionSystem.Raycast(p, d5, RaycastCallback,
out resBody, out hitNormal, out fraction);
if (result && resBody != rigidBody)
{
float maxLength = d.LengthSquared();
JVector collisionAt = p + fraction * d5;
JVector position = GetPosition(collider, collisionAt, hitNormal);
float ncLength = (position - p).LengthSquared();
//resBody.Tag = BodyTag.DrawMe;
//Debug.Log(fraction);
//Debug.Log((resBody as Collider).GameObject.name);
//World.AddBody(new RigidBody(new CylinderShape(1.0f, .25f)) { Position = p, IsStatic = true, PureCollider = true });
//World.AddBody(new RigidBody(new CylinderShape(2.5f, 0.5f)) { Position = end, IsStatic = true, PureCollider = true });
////World.AddBody(new RigidBody(new CylinderShape(1.0f, 1.0f)) { Position = p + d5, IsStatic = true, PureCollider = true });
//World.AddBody(new RigidBody(new CylinderShape(1.0f, 1.0f)) { Position = collisionAt, IsStatic = true, PureCollider = true });
return(maxLength < ncLength ? end : position);
}
return(end);
}
public override void ToBitStream(WriteOnlyBitStream packetStream, bool isInitialUpdate)
{
packetStream.Write1();
packetStream.Write(Position.X);
packetStream.Write(Position.Y);
packetStream.Write(Position.Z);
packetStream.Write(Rotation.X);
packetStream.Write(Rotation.Y);
packetStream.Write(Rotation.Z);
packetStream.Write(Rotation.W);
packetStream.Write(IsSupported);
packetStream.Write(IsOnRail);
if (LinearVelocity.LengthSquared() > JMath.Epsilon)
{
packetStream.Write1();
packetStream.Write(LinearVelocity.X);
packetStream.Write(LinearVelocity.Y);
packetStream.Write(LinearVelocity.Z);
}
else
{
packetStream.Write0();
}
if (AngularVelocity.LengthSquared() > JMath.Epsilon)
{
packetStream.Write1();
packetStream.Write(AngularVelocity.X);
packetStream.Write(AngularVelocity.Y);
packetStream.Write(AngularVelocity.Z);
}
else
{
packetStream.Write0();
}
if (LocalSpaceObjectId != 0)
{
packetStream.Write1();
packetStream.Write(LocalSpaceObjectId);
packetStream.Write(LocalPosition.X);
packetStream.Write(LocalPosition.Y);
packetStream.Write(LocalPosition.Z);
if (LocalLinearVelocity.LengthSquared() > JMath.Epsilon)
{
packetStream.Write1();
packetStream.Write(LocalLinearVelocity.X);
packetStream.Write(LocalLinearVelocity.Y);
packetStream.Write(LocalLinearVelocity.Z);
}
else
{
packetStream.Write0();
}
}
else
{
packetStream.Write0();
}
if (!isInitialUpdate)
{
packetStream.Write(Teleport);
}
}
public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
ref JMatrix orientation2, ref JVector position1, ref JVector position2,
out JVector p1, out JVector p2, out JVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
p1 = p2 = JVector.Zero;
JVector r = position1 - position2;
JVector w, v;
JVector supVertexA;
JVector rn, vn;
rn = JVector.Negate(r);
SupportMapTransformed(support1, ref orientation1, ref position1, ref rn, out supVertexA);
JVector supVertexB;
SupportMapTransformed(support2, ref orientation2, ref position2, ref r, out supVertexB);
v = supVertexA - supVertexB;
normal = JVector.Zero;
int maxIter = 15;
float distSq = v.LengthSquared();
float epsilon = 0.00001f;
while ((distSq > epsilon) && (maxIter-- != 0))
{
vn = JVector.Negate(v);
SupportMapTransformed(support1, ref orientation1, ref position1, ref vn, out supVertexA);
SupportMapTransformed(support2, ref orientation2, ref position2, ref v, out supVertexB);
w = supVertexA - supVertexB;
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, supVertexA, supVertexB);
}
if (simplexSolver.Closest(out v))
{
distSq = v.LengthSquared();
normal = v;
}
else
{
distSq = 0.0f;
}
}
simplexSolver.ComputePoints(out p1, out p2);
if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// Returns true if the point is inside the circle.
/// </summary>
/// <param name="point">The point.</param>
/// <returns>True if the point is inside the circle.</returns>
public override bool PointInsideLocal(JVector point)
{
return(point.LengthSquared() <= (radius * radius));
}
/// <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() - 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 = 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);
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 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>
/// 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);
}
// 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);
}
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();
}
}
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);
}
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
// CUSTOM: Added custom detection callbacks (primarily to accommodate actor movement on surfaces).
var callback1 = body1.ShouldGenerateContact;
var callback2 = body2.ShouldGenerateContact;
bool b1IsMulti = (body1.Shape is Multishape);
bool b2IsMulti = (body2.Shape is Multishape);
bool speculative = speculativeContacts ||
(body1.AreSpeculativeContactsEnabled || body2.AreSpeculativeContactsEnabled);
JVector point, normal;
float penetration;
if (!b1IsMulti && !b2IsMulti)
{
// CUSTOM: Added these callbacks (two rigid bodies).
if ((callback1 != null && !callback1(body2, null)) || (callback2 != null && !callback2(body1, null)))
{
return;
}
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)
{
// CUSTOM: Swapped callbacks here as well.
b1 = body2;
b2 = body1;
// A proper swap here (using a temporary variable) isn't necessary since, by this point, the other
// callback (attached to the static body) is intentionally not used.
callback2 = callback1;
}
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);
// CUSTOM: Added this callback (to allow specific triangle collisions to be ignored).
bool shouldCollideWith = ms is TriangleMeshShape tMesh && (callback2 == null ||
callback2(b1, tMesh.CurrentTriangle));
if (shouldCollideWith && XenoCollide.Detect(ms, b2.Shape, ref b1.orientation,
ref b2.orientation, ref b1.position, ref b2.position,
out point, out normal, out penetration))
{
JVector[] triangle = null;
FindSupportPoints(b1, b2, ms, b2.Shape, ref point, ref normal, out var point1, out var point2);
if (useTerrainNormal && ms is TerrainShape)
{
(ms as TerrainShape).CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
else if (useTriangleMeshNormal)
{
tMesh = ms as TriangleMeshShape;
triangle = tMesh.CurrentTriangle;
tMesh.CollisionNormal(out normal);
JVector.Transform(ref normal, ref b1.orientation, out normal);
}
RaiseCollisionDetected(b1, b2, ref point1, ref point2, ref normal, triangle, 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();
}
}