/// <summary>
/// By calling this method the shape inertia and mass is used.
/// </summary>
public void SetMassProperties()
{
this.inertia = Shape.inertia;
JMatrix.Inverse(ref inertia, out invInertia);
this.inverseMass = 1.0f / Shape.mass;
useShapeMassProperties = true;
}
private void OrientationCorrectionTorque(Matrix4 wantedOrientation, float scale)
{
#if false
// this is something like correction = wantedPosition - position
JMatrix q = JMatrix.Inverse(wantedOrientation) * testBody.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);
}
// 80.0f is the spring value "k"
testBody.AddTorque(JVector.Transform(axis, JMatrix.Inverse(testBody.InverseInertiaWorld)) * 80.0f);
// also apply some damping
testBody.AngularVelocity *= 0.9f;
#endif
Matrix4 q = Matrix4.Invert(wantedOrientation) * physicsObject.RigidBody.Orientation;
float x = q._12 - q._21;
float y = q._20 - q._02;
float z = q._01 - q._10;
float r = (float)Math.Sqrt(x * x + y * y + z * z);
float t = q._00 + q._11 + q._22;
float angle = (float)Math.Atan2(r, t - 1);
Vector3 axis = new Vector3(x, y, z) * angle;
if (r != 0.0f)
{
axis *= (1.0f / r);
}
Matrix4 inertiaWorld = Matrix4.Invert(physicsObject.RigidBody.InverseInertiaWorld);
inertiaWorld = Matrix4.Transpose(inertiaWorld);
axis = inertiaWorld.TransformDirection(axis);
physicsObject.RigidBody.AddTorque(axis * scale);
physicsObject.RigidBody.AngularVelocity *= 0.9f;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(double timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
JMatrix.Inverse(ref effectiveMass, out effectiveMass);
JMatrix orientationDifference;
JMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
JMatrix.Transpose(ref orientationDifference, out orientationDifference);
JMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
JVector axis;
double x = q.M32 - q.M23;
double y = q.M13 - q.M31;
double z = q.M21 - q.M12;
double r = JMath.Sqrt(x * x + y * y + z * z);
double t = q.M11 + q.M22 + q.M33;
double angle = (double)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);
}
if (!body2.IsStatic)
{
body2.angularVelocity += JVector.Transform(-1.0f * accumulatedImpulse, body2.invInertiaWorld);
}
}
/// <summary>
/// The engine used the given values for inertia and mass and ignores
/// the shape mass properties.
/// </summary>
/// <param name="inertia">The inertia/inverse inertia of the untransformed object.</param>
/// <param name="mass">The mass/inverse mass of the object.</param>
/// <param name="setAsInverseValues">Sets the InverseInertia and the InverseMass
/// to this values.</param>
public void SetMassProperties(JMatrix inertia, float mass, bool setAsInverseValues)
{
if (setAsInverseValues)
{
this.invInertia = inertia;
JMatrix.Inverse(ref inertia, out this.inertia);
this.inverseMass = mass;
}
else
{
this.inertia = inertia;
JMatrix.Inverse(ref inertia, out this.invInertia);
this.inverseMass = 1.0f / mass;
}
useShapeMassProperties = false;
Update();
}
/// <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);
}
}
// 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 PrepareForIteration(float timestep)
{
// send a ray from our feet position down.
// if we collide with something which is 0.05f units below our feets remember this!
RigidBody resultingBody = null;
JVector normal;
float frac;
var rayOrigin = Body1.Position + JVector.Down * (feetPosition - 0.1f);
bool result = JPhysics.World.CollisionSystem.Raycast(
rayOrigin,
JVector.Down,
(body, hitNormal, fraction) => body != Body1,
out resultingBody,
out normal,
out frac);
if (BodyWalkingOn != null)
{
contactPoint = rayOrigin + JVector.Down * frac;
localContactPoint = JVector.Transform(contactPoint - BodyWalkingOn.Position, JMatrix.Inverse(BodyWalkingOn.Orientation));
}
BodyWalkingOn = (result && frac <= 0.2f) ? resultingBody : null;
shouldIJump = TryJump && result && (frac <= 0.2f) && (Body1.LinearVelocity.Y < JumpVelocity);
}