/// <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);
}
/// <summary>
/// Subtracts two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The difference of both vectors.</returns>
#region public static JVector Subtract(JVector value1, JVector value2)
public static TSVector Subtract(TSVector value1, TSVector value2)
{
TSVector result;
TSVector.Subtract(ref value1, ref value2, out result);
return(result);
}
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();
}
/// <summary>
/// Adds a contact to the arbiter (threadsafe). No more than four contacts
/// are stored in the contactList. When adding a new contact
/// to the arbiter the existing are checked and the best are kept.
/// </summary>
/// <param name="point1">Point on body1. In world space.</param>
/// <param name="point2">Point on body2. In world space.</param>
/// <param name="normal">The normal pointing to body2.</param>
/// <param name="penetration">The estimated penetration depth.</param>
public Contact AddContact(TSVector point1, TSVector point2, TSVector normal, FP penetration,
ContactSettings contactSettings)
{
TSVector relPos1;
TSVector.Subtract(ref point1, ref body1.position, out relPos1);
int index;
lock (contactList)
{
if (this.contactList.Count == 4)
{
index = SortCachedPoints(ref relPos1, penetration);
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
index = GetCacheEntry(ref relPos1, contactSettings.breakThreshold);
if (index >= 0)
{
ReplaceContact(ref point1, ref point2, ref normal, penetration, index, contactSettings);
return(null);
}
else
{
Contact contact = Contact.Pool.GetNew();
contact.Initialize(body1, body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
contactList.Add(contact);
return(contact);
}
}
}
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);
}
/// <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));
}
}
/// <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);
}
/// <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 void GetNormal(out TSVector normal)
{
TSVector sum;
TSVector.Subtract(owner.points[indices.I1].position, owner.points[indices.I0].position, out sum);
TSVector.Subtract(owner.points[indices.I2].position, owner.points[indices.I0].position, out normal);
TSVector.Cross(sum, normal, out normal);
}
/// <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>
/// 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>
///// Transform the segment using the specified transformation.
///// </summary>
///// <param name="s">The segment to transform.</param>
///// <param name="transform">the transform to apply.</param>
///// <param name="output">Returns the transformed segment.</param>
//public static void Transform(ref SegmentShape s, ref Transform transform, out SegmentShape output)
//{
// TSVector.Transform(ref s.P1, ref transform.Combined, out output.P1);
// TSVector.Transform(ref s.P2, ref transform.Combined, out output.P2);
//}
/// <summary>
/// Intersects two line segments.
/// </summary>
/// <param name="sa">The first line segment.</param>
/// <param name="sb">The second line segment.</param>
/// <param name="scalarA">Returns a value between 0 and 1 indicating the point of intersection on the first segment.</param>
/// <param name="scalarB">Returns a value between 0 and 1 indicating the point of intersection on the second segment.</param>
/// <param name="p">Returns the point of intersection, common to both segments.</param>
/// <returns>Returns a value indicating whether there was an intersection.</returns>
public static bool Intersect(ref SegmentShape sa, ref SegmentShape sb, out FP scalarA, out FP scalarB, out TSVector p)
{
TSVector pa;
FP dist;
SegmentShape.ClosestPoints(ref sa, ref sb, out scalarA, out pa, out scalarB, out p);
dist = TSVector.Subtract(pa, p).sqrMagnitude;
return(dist < TSMath.Epsilon);
}
/// <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);
}
// sort cached points so most isolated points come first
//排序缓存点，因此大多数孤立点最先出现。
private int SortCachedPoints(ref TSVector realRelPos1, FP pen)
{
//calculate 4 possible cases areas, and take biggest area
//also need to keep 'deepest'
//计算4个可能的cases区域，并采取最大的区域也需要保持“最深”
int maxPenetrationIndex = -1;//最穿透的序号
FP maxPenetration = pen;
for (int i = 0; i < 4; i++)
{
if (contactList[i].penetration > maxPenetration)
{
maxPenetrationIndex = i;
maxPenetration = contactList[i].penetration;
}
}
FP res0 = 0, res1 = 0, res2 = 0, res3 = 0;
if (maxPenetrationIndex != 0)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[1].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res0 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 1)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[2].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res1 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 2)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[3].relativePos1, ref contactList[1].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res2 = cross.sqrMagnitude;
}
if (maxPenetrationIndex != 3)
{
TSVector a0; TSVector.Subtract(ref realRelPos1, ref contactList[0].relativePos1, out a0);
TSVector b0; TSVector.Subtract(ref contactList[2].relativePos1, ref contactList[1].relativePos1, out b0);
TSVector cross; TSVector.Cross(ref a0, ref b0, out cross);
res3 = cross.sqrMagnitude;
}
int biggestarea = MaxAxis(res0, res1, res2, res3);
return(biggestarea);
}
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>
/// 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;
}
/// <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);
}
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 dp;
TSVector.Subtract(ref body2.position, ref body1.position, 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 = dp;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
//jacobian[1] = -FP.One * (r1 % n);
jacobian[1] = FP.One * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
CBFrame.Utils.Logger.Debug("line148 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body1.isStatic)
{
body1.linearVelocity += body1.inverseMass * accumulatedImpulse * jacobian[0];
}
CBFrame.Utils.Logger.Debug("line153 body2.linearVelocity:" + body2.linearVelocity + ",body1.linearVelocity:" + body1.linearVelocity);
if (!body2.isStatic)
{
body2.linearVelocity += body2.inverseMass * accumulatedImpulse * jacobian[1];
}
CBFrame.Utils.Logger.Debug("line158 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 dp;
TSVector.Subtract(body2.position, body1.position, 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 = dp;
if (n.sqrMagnitude != FP.Zero)
{
n.Normalize();
}
jacobian[0] = -FP.One * n;
//jacobian[1] = -FP.One * (r1 % n);
jacobian[1] = FP.One * n;
//jacobian[3] = (r2 % n);
effectiveMass = body1.inverseMass + body2.inverseMass;
softnessOverDt = softness / timestep;
effectiveMass += softnessOverDt;
effectiveMass = FP.One / effectiveMass;
bias = deltaLength * biasFactor * (FP.One / timestep);
if (!body1.isStatic)
{
body1.ApplyImpulse(accumulatedImpulse * jacobian[0]);
}
if (!body2.isStatic)
{
body2.ApplyImpulse(accumulatedImpulse * jacobian[1]);
}
}
}
/// <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>
/// Calculates the inertia of a box with the sides of the multishape.
/// </summary>
public override void CalculateMassInertia()
{
geomCen = TSVector.zero;
// TODO: calc this right
inertia = TSMatrix.Identity;
TSVector size; TSVector.Subtract(boundingBox.max, boundingBox.min, out size);
mass = size.x * size.y * size.z;
inertia.M11 = (FP.One / (12 * FP.One)) * mass * (size.y * size.y + size.z * size.z);
inertia.M22 = (FP.One / (12 * FP.One)) * mass * (size.x * size.x + size.z * size.z);
inertia.M33 = (FP.One / (12 * FP.One)) * mass * (size.x * size.x + size.y * size.y);
}
/// <summary>
/// Create a bounding box appropriate for a child, based on a parents AABox
/// </summary>
/// <param name="aabb"></param>
/// <param name="child"></param>
/// <param name="result"></param>
#region private void CreateAABox(ref JBBox aabb, EChild child,out JBBox result)
private void CreateAABox(ref TSBBox aabb, EChild child, out TSBBox result)
{
TSVector dims;
TSVector.Subtract(ref aabb.max, ref aabb.min, out dims);
TSVector.Multiply(ref dims, FP.Half, out dims);
TSVector offset = TSVector.zero;
switch (child)
{
case EChild.PPP: offset = new TSVector(1, 1, 1); break;
case EChild.PPM: offset = new TSVector(1, 1, 0); break;
case EChild.PMP: offset = new TSVector(1, 0, 1); break;
case EChild.PMM: offset = new TSVector(1, 0, 0); break;
case EChild.MPP: offset = new TSVector(0, 1, 1); break;
case EChild.MPM: offset = new TSVector(0, 1, 0); break;
case EChild.MMP: offset = new TSVector(0, 0, 1); break;
case EChild.MMM: offset = new TSVector(0, 0, 0); break;
default:
System.Diagnostics.Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = new TSBBox();
result.min = new TSVector(offset.x * dims.x, offset.y * dims.y, offset.z * dims.z);
TSVector.Add(ref result.min, ref aabb.min, out result.min);
TSVector.Add(ref result.min, ref dims, out result.max);
// expand it just a tiny bit just to be safe!
FP extra = FP.EN5;
TSVector temp; TSVector.Multiply(ref dims, extra, out temp);
TSVector.Subtract(ref result.min, ref temp, out result.min);
TSVector.Add(ref result.max, ref temp, out result.max);
}
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);
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);
}
public MyHingeJoint(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
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)body1, (RigidBody)body2, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
}
private int GetCacheEntry(ref TSVector realRelPos1, FP contactBreakThreshold)
{
FP shortestDist = contactBreakThreshold * contactBreakThreshold;
int size = contactList.Count;
int nearestPoint = -1;
for (int i = 0; i < size; i++)
{
TSVector diffA; TSVector.Subtract(ref contactList[i].relativePos1, ref realRelPos1, out diffA);
FP distToManiPoint = diffA.sqrMagnitude;
if (distToManiPoint < shortestDist)
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return(nearestPoint);
}