/// <summary>
/// Adds two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The sum of both vectors.</returns>
#region public static void Add(JVector value1, JVector value2)
public static TSVector Add(TSVector value1, TSVector value2)
{
TSVector result;
TSVector.Add(ref value1, ref value2, 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;
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 bool OverlapTest(ref CapsuleShape capsule, ref TSVector p1, ref TSVector p2,
ref SphereShape sphere, ref TSVector sphereCenter,
ref TSVector pa, ref TSVector pb, ref TSVector normal, ref FP penetration)
{
SegmentShape cap = SegmentShape.Pool.GetNew();
cap.P1 = p1;
cap.P2 = p2;
TSVector v;
FP r2 = capsule.Radius + sphere.Radius;
r2 *= r2;
FP sb;
cap.ClosestPointTo(ref sphereCenter, out sb, out pb);
SegmentShape.Pool.GiveBack(cap);
TSVector.Subtract(ref sphereCenter, ref pb, out normal);
if (normal.sqrMagnitude - r2 >= TSMath.Epsilon)
{
return(false);
}
penetration = (capsule.radius + sphere.radius) - normal.magnitude;
normal.Normalize();
TSVector.Multiply(ref normal, -sphere.Radius, out v);
TSVector.Add(ref sphereCenter, ref v, out pa);
TSVector.Multiply(ref normal, capsule.Radius, out v);
TSVector.Add(ref pb, ref v, out pb);
TSVector.Negate(ref normal, out normal);
return(true);
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal,
out TSVector point1, out TSVector point2)
{
TSVector mn;
TSVector.Negate(ref normal, out mn);
TSVector sA;
SupportMapping(body1, shape1, ref mn, out sA);
TSVector sB;
SupportMapping(body2, shape2, ref normal, out sB);
TSVector.Subtract(ref sA, ref point, out sA);
TSVector.Subtract(ref sB, ref point, out sB);
FP dot1 = TSVector.Dot(ref sA, ref normal);
FP dot2 = TSVector.Dot(ref sB, ref normal);
TSVector.Multiply(ref normal, dot1, out sA);
TSVector.Multiply(ref normal, dot2, out sB);
TSVector.Add(ref point, ref sA, out point1);
TSVector.Add(ref point, ref sB, out point2);
}
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();
}
}
/// <summary>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(TSVector force, TSVector pos)
{
TSVector.Add(ref this.force, ref force, out this.force);
TSVector.Subtract(ref pos, ref this.position, out pos);
TSVector.Cross(ref pos, ref force, out pos);
TSVector.Add(ref pos, ref this.torque, out this.torque);
}
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);
}
/// <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);
}
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>
/// Adds a force to the center of the body. The force gets applied
/// the next time <see cref="World.Step"/> is called. The 'impact'
/// of the force depends on the time it is applied to a body - so
/// the timestep influences the energy added to the body.
/// </summary>
/// <param name="force">The force to add next <see cref="World.Step"/>.</param>
/// <param name="pos">The position where the force is applied.</param>
public void AddForce(TSVector force, TSVector pos)
{
TSVector.Add(this.force, force, out this.force);
TSVector.Subtract(pos, this.position, out pos);
TSVector.Cross(pos, force, out pos);
TSVector.Add(pos, this.torque, out this.torque);
}
public CharacterJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
RigidBody rigid1 = (RigidBody)body1;
RigidBody rigid2 = (RigidBody)body2;
fixedAngle = new FixedAngle(rigid1, rigid2);
pointOnLine = new PointOnLine(rigid1, rigid2, rigid1.position, rigid2.position);
firstBody = body1;
secondBody = body2;
hingeA = hingeAxis;
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body2, (RigidBody)body1, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
//UnityEngine.CharacterJoint;
Activate();
}
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 expandVector;
TSVector.Normalize(direction, out expandVector);
TSVector.Multiply(expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = TSVector.Dot(points[0], direction);
FP dot = TSVector.Dot(points[1], direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = TSVector.Dot(points[2], direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
TSVector.Add(points[minIndex], expandVector, 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;
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>
/// Recalculates the axis aligned bounding box and the inertia
/// values in world space.
/// </summary>
public virtual void Update()
{
if (isParticle)
{
this.inertia = TSMatrix.Zero;
this.invInertia = this.invInertiaWorld = TSMatrix.Zero;
this.invOrientation = this.orientation = TSMatrix.Identity;
this.boundingBox = shape.boundingBox;
TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
angularVelocity.MakeZero();
}
else
{
// Given: Orientation, Inertia
TSMatrix.Transpose(ref orientation, out invOrientation);
this.Shape.GetBoundingBox(ref orientation, out boundingBox);
TSVector.Add(ref boundingBox.min, ref this.position, out boundingBox.min);
TSVector.Add(ref boundingBox.max, ref this.position, out boundingBox.max);
if (!isStatic)
{
TSMatrix.Multiply(ref invOrientation, ref invInertia, out invInertiaWorld);
TSMatrix.Multiply(ref invInertiaWorld, ref orientation, out invInertiaWorld);
}
}
}
public void SupportCenter(out TSVector center)
{
center = owner.points[indices.I0].position;
TSVector.Add(center, owner.points[indices.I1].position, out center);
TSVector.Add(center, owner.points[indices.I2].position, out center);
TSVector.Multiply(center, FP.One / (3 * FP.One), out center);
}
/// <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>
/// 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);
}
private void IntegrateCallback(object obj)
{
RigidBody body = obj as RigidBody;
TSVector temp;
TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
TSVector.Add(ref temp, ref body.position, out body.position);
if (!(body.isParticle))
{
//exponential map
TSVector axis;
FP angle = body.angularVelocity.magnitude;
FP halfTimeStep = FP.Half * timestep;
FP halfTimeStepAngle = halfTimeStep * angle;
if (angle < FP.EN3)
{
// use Taylor's expansions of sync function
// axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
//TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
}
TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
TSQuaternion ornA;
TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);
TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);
dorn.Normalize();
TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
}
body.linearVelocity /= (1 + timestep * body.linearDrag);
body.angularVelocity /= (1 + timestep * body.angularDrag);
/*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
* TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
*
* if ((body.Damping & RigidBody.DampingType.Angular) != 0)
* TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/
body.Update();
if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
{
body.SweptExpandBoundingBox(timestep);
}
}
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>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(TSVector impulse)
{
if (this.isStatic || impulse.IsZero())
{
return;
}
//UnityEngine.Debug.Log($"ApplyImpulse {impulse}");
TSVector temp;
TSVector.Multiply(impulse, inverseMass, out temp);
TSVector.Add(linearVelocity, temp, out linearVelocity);
}
/// <summary>
/// Applies an impulse on the center of the body. Changing
/// linear velocity.
/// </summary>
/// <param name="impulse">Impulse direction and magnitude.</param>
public void ApplyImpulse(TSVector impulse)
{
if (this.isStatic)
{
return;
}
TSVector temp;
TSVector.Multiply(ref impulse, inverseMass, out temp);
TSVector.Add(ref linearVelocity, ref temp, out linearVelocity);
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector temp1, temp2 = TSVector.zero;
for (int i = 0; i < shapes.Count; i++)
{
shapes[i].SupportMapping(ref direction, out temp1);
TSVector.Add(ref temp1, ref temp2, out temp2);
}
TSVector.Subtract(ref temp2, ref shifted, out result);
}
/// <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);
}
private HingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector pos1 = position; TSVector.Add(ref pos1, ref hingeAxis, out pos1);
TSVector pos2 = position; TSVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos1);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos2);
Activate();
}
/// <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>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = TSVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += TSVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
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;
}
TSVector temp;
CBFrame.Utils.Logger.Debug("line195 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
TSVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
TSVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line201 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
TSVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
TSVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
CBFrame.Utils.Logger.Debug("line206 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
}
/// <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);
}