/// <summary>
/// By calling this method the shape inertia and mass is used.
/// </summary>
public void SetMassProperties()
{
this.inertia = Shape.inertia;
TSMatrix.Inverse(ref inertia, out invInertia);
this.inverseMass = FP.One / Shape.mass;
useShapeMassProperties = true;
}
/// <summary>
/// Calculates the inverse of a give matrix.
/// </summary>
/// <param name="matrix">The matrix to invert.</param>
/// <returns>The inverted JMatrix.</returns>
#region public static JMatrix Inverse(JMatrix matrix)
public static TSMatrix Inverse(TSMatrix matrix)
{
TSMatrix result;
TSMatrix.Inverse(ref matrix, out result);
return(result);
}
/// <summary>
/// By calling this method the shape inertia and mass is used.
/// </summary>
public void SetMassProperties()
{
TSLinearDrag = TrueSyncManager.Config.linearDrag;
TSAngularDrag = TrueSyncManager.Config.angularDrag;
this.inertia = Shape.inertia;
TSMatrix.Inverse(ref inertia, out invInertia);
this.inverseMass = FP.One / Shape.mass;
useShapeMassProperties = true;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
effectiveMass = body1.invInertiaWorld + body2.invInertiaWorld;
softnessOverDt = softness / timestep;
effectiveMass.M11 += softnessOverDt;
effectiveMass.M22 += softnessOverDt;
effectiveMass.M33 += softnessOverDt;
TSMatrix.Inverse(ref effectiveMass, out effectiveMass);
TSMatrix orientationDifference;
TSMatrix.Multiply(ref initialOrientation1, ref initialOrientation2, out orientationDifference);
TSMatrix.Transpose(ref orientationDifference, out orientationDifference);
TSMatrix q = orientationDifference * body2.invOrientation * body1.orientation;
TSVector axis;
FP x = q.M32 - q.M23;
FP y = q.M13 - q.M31;
FP z = q.M21 - q.M12;
FP r = TSMath.Sqrt(x * x + y * y + z * z);
FP t = q.M11 + q.M22 + q.M33;
FP angle = FP.Atan2(r, t - 1);
axis = new TSVector(x, y, z) * angle;
if (r != FP.Zero)
{
axis = axis * (FP.One / r);
}
bias = axis * biasFactor * (-FP.One / timestep);
// Apply previous frame solution as initial guess for satisfying the constraint.
if (!body1.IsStatic)
{
body1.angularVelocity += TSVector.Transform(accumulatedImpulse, body1.invInertiaWorld);
}
if (!body2.IsStatic)
{
body2.angularVelocity += TSVector.Transform(-FP.One * 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(TSMatrix inertia, FP mass, bool setAsInverseValues)
{
if (setAsInverseValues)
{
if (!isParticle)
{
this.invInertia = inertia;
TSMatrix.Inverse(ref inertia, out this.inertia);
}
this.inverseMass = mass;
}
else
{
if (!isParticle)
{
this.inertia = inertia;
TSMatrix.Inverse(ref inertia, out this.invInertia);
}
this.inverseMass = FP.One / mass;
}
useShapeMassProperties = false;
Update();
}
public FP GetSphereDistance(ref BoxShape box, ref TSVector boxPosition, ref TSMatrix boxOrientation, ref TSVector sphereCenter, FP radius,
ref TSVector pointOnBox, ref TSVector pointOnSphere, ref TSVector normal)
{
FP margins;
bounds[0] = TSVector.Negate(box.halfSize);
bounds[1] = box.halfSize;
margins = FP.Zero; //TODO: box.Margin; //also add sphereShape margin?
boundsVec[0] = bounds[0];
boundsVec[1] = bounds[1];
TSVector marginsVec = TSVector.one * margins;
// add margins
bounds[0] += marginsVec;
bounds[1] -= marginsVec;
TSVector prel, v3P;
FP sep = FP.MaxValue;
FP sepThis;
// convert point in local space
TSVector.Subtract(ref sphereCenter, ref boxPosition, out sphereCenter);
TSMatrix invBoxOrientation;
TSMatrix.Inverse(ref boxOrientation, out invBoxOrientation);
TSVector.Transform(ref sphereCenter, ref invBoxOrientation, out prel);
bool found = false;
v3P = prel;
for (int i = 0; i < 6; i++)
{
int j = i < 3 ? 0 : 1;
if ((sepThis = (TSVector.Dot(v3P - bounds[j], n[i]))) > FP.Zero)
{
v3P = v3P - n[i] * sepThis;
found = true;
}
}
if (found)
{
bounds[0] = boundsVec[0];
bounds[1] = boundsVec[1];
normal = TSVector.Normalize(prel - v3P);
pointOnBox = v3P + normal * margins;
pointOnSphere = prel - normal * radius;
if ((TSVector.Dot(pointOnSphere - pointOnBox, normal)) > FP.Zero)
{
return(FP.One);
}
FP seps2 = (pointOnBox - pointOnSphere).sqrMagnitude;
//if this fails, fallback into deeper penetration case, below
if (seps2 > TSMath.Epsilon)
{
// transform back in world space
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
TSVector.Transform(ref pointOnSphere, ref boxOrientation, out pointOnSphere);
TSVector.Add(ref pointOnSphere, ref boxPosition, out pointOnSphere);
sep = -TSMath.Sqrt(seps2);
normal = (pointOnBox - pointOnSphere);
normal *= FP.One / sep;
}
return(sep);
}
return(FP.One);
}
public FP GetSphereDistance(ref BoxShape box, ref TSVector boxPosition, ref TSMatrix boxOrientation, ref TSVector sphereCenter, FP radius,
ref TSVector pointOnBox, ref TSVector pointOnSphere, ref TSVector normal)
{
TSVector boxHalfSize = box.halfSize;
TSVector prel;
// convert sphere center into box local space
TSVector.Subtract(ref sphereCenter, ref boxPosition, out prel);
TSMatrix invBoxOrientation;
TSMatrix.Inverse(ref boxOrientation, out invBoxOrientation);
TSVector.Transform(ref prel, ref invBoxOrientation, out prel);
pointOnBox = prel;
bool outSide = false;
//x
if (prel.x < -boxHalfSize.x)
{
outSide = true;
pointOnBox.x = -boxHalfSize.x;
}
else if (prel.x > boxHalfSize.x)
{
outSide = true;
pointOnBox.x = boxHalfSize.x;
}
//y
if (prel.y < -boxHalfSize.y)
{
outSide = true;
pointOnBox.y = -boxHalfSize.y;
}
else if (prel.y > boxHalfSize.y)
{
outSide = true;
pointOnBox.y = boxHalfSize.y;
}
//z
if (prel.z < -boxHalfSize.z)
{
outSide = true;
pointOnBox.z = -boxHalfSize.z;
}
else if (prel.z > boxHalfSize.z)
{
outSide = true;
pointOnBox.z = boxHalfSize.z;
}
if (outSide)
{
normal = prel - pointOnBox;
FP dist2 = normal.sqrMagnitude;
if (dist2 > radius * radius)
{
return(FP.One);
}
FP distance;
if (dist2 <= TSMath.Epsilon)
{
distance = -GetSpherePenetration(boxHalfSize, prel, ref pointOnBox, out normal);
}
else
{
distance = normal.magnitude;
normal /= distance;
}
// transform back in world space
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
pointOnSphere = prel - normal * radius;
TSVector.Transform(ref pointOnSphere, ref boxOrientation, out pointOnSphere);
TSVector.Add(ref pointOnSphere, ref boxPosition, out pointOnSphere);
normal = TSVector.Transform(normal, boxOrientation);
normal = TSVector.Negate(normal);
normal.Normalize();
return(distance - radius);
}
else
{
FP distance;
distance = -GetSpherePenetration(boxHalfSize, prel, ref pointOnBox, out normal);
normal = TSVector.Transform(normal, boxOrientation);
normal = TSVector.Negate(normal);
pointOnSphere = sphereCenter + normal * radius;
TSVector.Transform(ref pointOnBox, ref boxOrientation, out pointOnBox);
TSVector.Add(ref pointOnBox, ref boxPosition, out pointOnBox);
return(distance - radius);
}
}
