/// <summary>
/// Transforms a vector by the given matrix.
/// </summary>
/// <param name="position">The vector to transform.</param>
/// <param name="matrix">The transform matrix.</param>
/// <returns>The transformed vector.</returns>
#region public static JVector Transform(JVector position, JMatrix matrix)
public static TSVector Transform(TSVector position, TSMatrix matrix)
{
TSVector result;
TSVector.Transform(ref position, ref matrix, out result);
return(result);
}
/// <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)
{
TSVector.Add(ref realRelPos1, ref body1.position, out p1);
}
else
{
TSVector.Transform(ref realRelPos1, ref body1.orientation, out p1);
TSVector.Add(ref p1, ref body1.position, out p1);
}
if (body2IsMassPoint)
{
TSVector.Add(ref realRelPos2, ref body2.position, out p2);
}
else
{
TSVector.Transform(ref realRelPos2, ref body2.orientation, out p2);
TSVector.Add(ref p2, ref body2.position, out p2);
}
TSVector dist; TSVector.Subtract(ref p1, ref p2, out dist);
penetration = TSVector.Dot(ref dist, ref normal);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
TriangleMeshShape triangle = this.Shape1 as TriangleMeshShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of sphere in world coordinates -> center1
triangle.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of triangle in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector[] vertices = triangle.Vertices;
TSVector.Transform(ref vertices[0], ref orientation1, out vertices[0]);
TSVector.Add(ref position1, ref vertices[0], out vertices[0]);
TSVector.Transform(ref vertices[1], ref orientation1, out vertices[1]);
TSVector.Add(ref position1, ref vertices[1], out vertices[1]);
TSVector.Transform(ref vertices[2], ref orientation1, out vertices[2]);
TSVector.Add(ref position1, ref vertices[2], out vertices[2]);
return(Collide(center2, sphere.radius, ref vertices, ref point, ref point1, ref point2, ref normal, ref penetration));
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
FP jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + softnessScalar + bias);
accumulatedImpulse += lambda;
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += TSVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
//body2.linearVelocity = Vector3.zero;
body2.angularVelocity += TSVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
}
public void Rotate(TSMatrix orientation, TSVector center)
{
for (int i = 0; i < points.Count; i++)
{
points[i].position = TSVector.Transform(points[i].position - center, orientation);
}
}
private void IntegrateForces()
{
for (int index = 0, length = rigidBodies.Count; index < length; index++)
{
RigidBody body = rigidBodies[index];
if (!body.isStatic && body.IsActive)
{
TSVector temp;
TSVector.Multiply(ref body.force, body.inverseMass * timestep, out temp);
TSVector.Add(ref temp, ref body.linearVelocity, out body.linearVelocity);
if (!(body.isParticle))
{
TSVector.Multiply(ref body.torque, timestep, out temp);
TSVector.Transform(ref temp, ref body.invInertiaWorld, out temp);
TSVector.Add(ref temp, ref body.angularVelocity, out body.angularVelocity);
}
if (body.affectedByGravity)
{
TSVector.Multiply(ref gravity, timestep, out temp);
TSVector.Add(ref body.linearVelocity, ref temp, out body.linearVelocity);
}
}
body.force.MakeZero();
body.torque.MakeZero();
}
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule = this.Shape1 as CapsuleShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of capsule in world coordinates -> center1
capsule.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
return(OverlapTest(ref capsule, ref aP1, ref aP2, ref sphere, ref center2,
ref point1, ref point2, ref normal, ref penetration));
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector.Transform(ref direction, ref shapes[currentShape].invOrientation, out result);
shapes[currentShape].Shape.SupportMapping(ref direction, out result);
TSVector.Transform(ref result, ref shapes[currentShape].orientation, out result);
TSVector.Add(ref result, ref shapes[currentShape].position, out result);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
FP jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
accumulatedImpulse += lambda;
CBFrame.Utils.Logger.Debug("line151 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += TSVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line157 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
body2.angularVelocity += TSVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line163 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <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 FP fraction)
public override bool Raycast(RigidBody body, TSVector rayOrigin, TSVector rayDirection, out TSVector normal, out FP fraction)
{
fraction = FP.MaxValue; normal = TSVector.zero;
if (!body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection))
{
return(false);
}
if (body.Shape is Multishape)
{
Multishape ms = (body.Shape as Multishape).RequestWorkingClone();
TSVector tempNormal; FP tempFraction;
bool multiShapeCollides = false;
TSVector transformedOrigin; TSVector.Subtract(ref rayOrigin, ref body.position, out transformedOrigin);
TSVector.Transform(ref transformedOrigin, ref body.invOrientation, out transformedOrigin);
TSVector transformedDirection; TSVector.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);
TSVector.Transform(ref tempNormal, ref body.orientation, out tempNormal);
tempNormal.Negate();
}
else if (useTriangleMeshNormal && ms is TriangleMeshShape)
{
(ms as TriangleMeshShape).CollisionNormal(out tempNormal);
TSVector.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));
}
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
BoxShape box1 = this.Shape1 as BoxShape;
BoxShape box2 = this.Shape2 as BoxShape;
// Get the center of box1 in world coordinates -> center1
box1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of box2 in world coordinates -> center2
box2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
// TODO: box-box collision test
return(true);
}
private void SupportMapping(RigidBody body, Shape workingShape, ref TSVector direction, out TSVector result)
{
TSVector.Transform(ref direction, ref body.invOrientation, out result);
workingShape.SupportMapping(ref result, out result);
TSVector.Transform(ref result, ref body.orientation, out result);
TSVector.Add(ref result, ref body.position, out result);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
BoxShape box = this.Shape1 as BoxShape;
SphereShape sphere = this.Shape2 as SphereShape;
// Get the center of box in world coordinates -> center1
box.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere in world coordinates -> center2
sphere.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
FP dist = GetSphereDistance(ref box, ref center1, ref orientation1, ref center2, sphere.radius,
ref point1, ref point2, ref normal);
if (dist < TSMath.Epsilon)
{
penetration = -dist;
return(true);
}
return(false);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude - distance;
if (behavior == DistanceBehavior.LimitMaximumDistance && deltaLength <= FP.Zero)
{
skipConstraint = true;
}
else if (behavior == DistanceBehavior.LimitMinimumDistance && deltaLength >= FP.Zero)
{
skipConstraint = true;
}
else
{
skipConstraint = false;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero) n.Normalize();
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
}
/// <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 TSVector point1, ref TSVector point2, ref TSVector n,
FP 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;
TSVector.Subtract(ref p1, ref body1.position, out relativePos1);
TSVector.Subtract(ref p2, ref body2.position, out relativePos2);
TSVector.Transform(ref relativePos1, ref body1.invOrientation, out realRelPos1);
TSVector.Transform(ref relativePos2, ref body2.invOrientation, out realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
body1IsMassPoint = body1.isParticle;
body2IsMassPoint = body2.isParticle;
// Material Properties
if (newContact)
{
treatBody1AsStatic = body1.isStatic;
treatBody2AsStatic = body2.isStatic;
CBFrame.Utils.Logger.Debug("line812 body2.isStatic:" + body2.isStatic + ",body1.isStatic:" + body1.isStatic);
accumulatedNormalImpulse = FP.Zero;
accumulatedTangentImpulse = FP.Zero;
lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
staticFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Max(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Max(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
staticFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
dynamicFriction = TSMath.Min(body1.staticFriction, body2.staticFriction);
restitution = TSMath.Min(body1.restitution, body2.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
staticFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
dynamicFriction = (body1.staticFriction + body2.staticFriction) * FP.Half;
restitution = (body1.restitution + body2.restitution) * FP.Half;
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, TSVector anchor)
: base(body1, body2)
{
TSVector.Subtract(ref anchor, ref body1.position, out localAnchor1);
TSVector.Subtract(ref anchor, ref body2.position, out localAnchor2);
TSVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
TSVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
TSVector l = TSVector.Transform(lineNormal, body1.orientation);
l.Normalize();
TSVector t = (p1 - p2) % l;
if (t.sqrMagnitude != FP.Zero)
{
t.Normalize();
}
t = t % l;
jacobian[0] = t; // linearVel Body1
jacobian[1] = (r1 + p2 - p1) % t; // angularVel Body1
jacobian[2] = -FP.One * t; // linearVel Body2
jacobian[3] = -FP.One * r2 % t; // angularVel Body2
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
if (effectiveMass != 0)
{
effectiveMass = FP.One / effectiveMass;
}
bias = -(l % (p2 - p1)).magnitude * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line122 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line128 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line134 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Gets the axis aligned bounding box of the orientated shape.
/// </summary>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="box">The axis aligned bounding box of the shape.</param>
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
TSMatrix abs; TSMath.Absolute(ref orientation, out abs);
TSVector temp;
TSVector.Transform(ref halfSize, ref abs, out temp);
box.max = temp;
TSVector.Negate(ref temp, out box.min);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
SphereShape sphere1 = this.Shape1 as SphereShape;
SphereShape sphere2 = this.Shape2 as SphereShape;
// Get the center of sphere1 in world coordinates -> center1
sphere1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of sphere2 in world coordinates -> center2
sphere2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector c12 = TSVector.Subtract(center1, center2);
FP dot = TSVector.Dot(c12, c12);
FP r = sphere1.radius + sphere2.radius;
if (dot <= r * r)
{
//Get the unit direction from the first sphere's center to the second sphere's center.
TSVector.Subtract(ref center2, ref center1, out normal);
if (normal.sqrMagnitude < TSMath.Epsilon)
{
// Spheres are on the same position, we can choose any normal vector.
// Possibly it would be better to consider the object movement (velocities), but
// it is not important since this case should be VERY rare.
normal = TSVector.forward;
}
else
{
normal = normal.normalized;
}
FP r1 = sphere1.radius;
FP r2 = sphere2.radius;
point1 = normal * r1 + center1;
point2 = TSVector.Negate(normal) * r2 + center2;
TSVector.Negate(ref normal, out normal);
penetration = r - TSMath.Sqrt(dot);
return(true);
}
return(false);
}
/// <summary>
/// Constraints a point on a body to be fixed on a line
/// which is fixed on another body.
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="lineStartPointBody1"></param>
/// <param name="lineDirection"></param>
/// <param name="pointBody2"></param>
public PointOnLine(RigidBody body1, RigidBody body2,
TSVector lineStartPointBody1, TSVector pointBody2) : base(body1, body2)
{
TSVector.Subtract(ref lineStartPointBody1, ref body1.position, out localAnchor1);
TSVector.Subtract(ref pointBody2, ref body2.position, out localAnchor2);
TSVector.Transform(ref localAnchor1, ref body1.invOrientation, out localAnchor1);
TSVector.Transform(ref localAnchor2, ref body2.invOrientation, out localAnchor2);
lineNormal = TSVector.Normalize(lineStartPointBody1 - pointBody2);
}
/// <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, TSVector anchor1, TSVector anchor2)
: base(body1, body2)
{
TSVector.Subtract(anchor1, body1.position, out localAnchor1);
TSVector.Subtract(anchor2, body2.position, out localAnchor2);
TSVector.Transform(localAnchor1, body1.invOrientation, out localAnchor1);
TSVector.Transform(localAnchor2, body2.invOrientation, out localAnchor2);
distance = (anchor1 - anchor2).magnitude;
}
public void Transform(ref TSMatrix orientation)
{
TSVector halfExtents = FP.Half * (max - min);
TSVector center = FP.Half * (max + min);
TSVector.Transform(ref center, ref orientation, out center);
TSMatrix abs; TSMath.Absolute(ref orientation, out abs);
TSVector.Transform(ref halfExtents, ref abs, out halfExtents);
max = center + halfExtents;
min = center - halfExtents;
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref localAnchor1, ref body1.orientation, out r1);
TSVector.Transform(ref localAnchor2, ref body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(ref body1.position, ref r1, out p1);
TSVector.Add(ref body2.position, ref r2, out p2);
TSVector.Subtract(ref p2, ref p1, out dp);
FP deltaLength = dp.magnitude;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line117 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line123 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[2];
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line129 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <summary>
/// Called once before iteration starts.
/// </summary>
/// <param name="timestep">The 5simulation timestep</param>
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(localAnchor1, body1.orientation, out r1);
TSVector.Transform(localAnchor2, body2.orientation, out r2);
TSVector p1, p2, dp;
TSVector.Add(body1.position, r1, out p1);
TSVector.Add(body2.position, r2, out p2);
TSVector.Subtract(p2, p1, out dp);
FP deltaLength = dp.magnitude;
TSVector n = p2 - p1;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
jacobian[1] = -FP.One * (r1 % n);
jacobian[2] = FP.One * n;
jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass
+ TSVector.Transform(jacobian[1], body1.invInertiaWorld) * jacobian[1]
+ TSVector.Transform(jacobian[3], body2.invInertiaWorld) * jacobian[3];
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.ApplyImpulse(accumulatedImpulse * jacobian[0]);
body1.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(accumulatedImpulse * jacobian[2]);
body2.angularVelocity += TSVector.Transform(accumulatedImpulse * jacobian[3], body2.invInertiaWorld);
}
}
/// <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>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(TSVector impulse, TSVector relativePosition)
{
if (this.isStatic || impulse.IsZero())
{
return;
}
TSVector temp;
TSVector.Multiply(impulse, inverseMass, out temp);
TSVector.Add(linearVelocity, temp, out linearVelocity);
TSVector.Cross(relativePosition, impulse, out temp);
TSVector.Transform(temp, invInertiaWorld, out temp);
TSVector.Add(angularVelocity, temp, out angularVelocity);
}
/// <summary>
/// Applies an impulse on the specific position. Changing linear
/// and angular velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
/// <param name="relativePosition">The position where the impulse gets applied
/// in Body coordinate frame.</param>
public void ApplyImpulse(TSVector impulse, TSVector relativePosition)
{
if (this.isStatic)
{
return;
}
TSVector temp;
TSVector.Multiply(ref impulse, inverseMass, out temp);
TSVector.Add(ref linearVelocity, ref temp, out linearVelocity);
TSVector.Cross(ref relativePosition, ref impulse, out temp);
TSVector.Transform(ref temp, ref invInertiaWorld, out temp);
TSVector.Add(ref angularVelocity, ref temp, out angularVelocity);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv =
body1.linearVelocity * jacobian[0] +
body1.angularVelocity * jacobian[1] +
body2.linearVelocity * jacobian[2] +
body2.angularVelocity * jacobian[3];
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = TSMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
CBFrame.Utils.Logger.Debug("line202 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * lambda * jacobian[0];
body1.angularVelocity += TSVector.Transform(lambda * jacobian[1], body1.invInertiaWorld);
}
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * lambda * jacobian[2];
body2.angularVelocity += TSVector.Transform(lambda * jacobian[3], body2.invInertiaWorld);
}
CBFrame.Utils.Logger.Debug("line213 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
public override bool IsColliding(ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2,
out TSVector point, out TSVector point1, out TSVector point2, out TSVector normal, out FP penetration)
{
// Used variables
TSVector center1, center2;
// Initialization of the output
point = point1 = point2 = normal = TSVector.zero;
penetration = FP.Zero;
CapsuleShape capsule1 = this.Shape1 as CapsuleShape;
CapsuleShape capsule2 = this.Shape2 as CapsuleShape;
// Get the center of box1 in world coordinates -> center1
capsule1.SupportCenter(out center1);
TSVector.Transform(ref center1, ref orientation1, out center1);
TSVector.Add(ref position1, ref center1, out center1);
// Get the center of box2 in world coordinates -> center2
capsule2.SupportCenter(out center2);
TSVector.Transform(ref center2, ref orientation2, out center2);
TSVector.Add(ref position2, ref center2, out center2);
TSVector aP1, aP2, axisA;
TSVector halfAxisVecA = TSVector.up * FP.Half * capsule1.length;
TSVector.Transform(ref halfAxisVecA, ref orientation1, out axisA);
TSVector.Add(ref center1, ref axisA, out aP1);
TSVector.Negate(ref axisA, out axisA);
TSVector.Add(ref center1, ref axisA, out aP2);
//UnityEngine.Debug.DrawLine(aP1.ToVector(), aP2.ToVector(), UnityEngine.Color.red, 1);
TSVector bP1, bP2, axisB;
TSVector halfAxisVecB = TSVector.up * FP.Half * capsule2.length;
TSVector.Transform(ref halfAxisVecB, ref orientation2, out axisB);
TSVector.Add(ref center2, ref axisB, out bP1);
TSVector.Negate(ref axisB, out axisB);
TSVector.Add(ref center2, ref axisB, out bP2);
//UnityEngine.Debug.DrawLine(bP1.ToVector(), bP2.ToVector(), UnityEngine.Color.blue, 1);
return(DoOverlapTest(ref capsule1, ref aP1, ref aP2,
ref capsule2, ref bP1, ref bP2, TSVector.zero, ref axisA, ref axisB,
ref point1, ref point2, ref normal, ref penetration));
}
public override void MakeHull(ref List <TSVector> triangleList, int generationThreshold)
{
List <TSVector> triangles = new List <TSVector>();
for (int i = 0; i < shapes.Length; i++)
{
shapes[i].Shape.MakeHull(ref triangles, 4);
for (int e = 0; e < triangles.Count; e++)
{
TSVector pos = triangles[e];
TSVector.Transform(ref pos, ref shapes[i].orientation, out pos);
TSVector.Add(ref pos, ref shapes[i].position, out pos);
triangleList.Add(pos);
}
triangles.Clear();
}
}
