public void GetSearchDirection(Vec2 result)
{
switch (Count)
{
case 1:
result.Set(m_v1.W).NegateLocal();
return;
case 2:
e12.Set(m_v2.W).SubLocal(m_v1.W);
// use out for a temp variable real quick
result.Set(m_v1.W).NegateLocal();
float sgn = Vec2.Cross(e12, result);
if (sgn > 0f)
{
// Origin is left of e12.
Vec2.CrossToOutUnsafe(1f, e12, result);
return;
}
else
{
// Origin is right of e12.
Vec2.CrossToOutUnsafe(e12, 1f, result);
return;
}
default:
Debug.Assert(false);
result.SetZero();
return;
}
}
public float MotorMass; // effective mass for motor/limit translational constraint.
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
LocalAnchorA = new Vec2(def.LocalAnchorA);
LocalAnchorB = new Vec2(def.LocalAnchorB);
LocalXAxisA = new Vec2(def.LocalAxisA);
LocalXAxisA.Normalize();
LocalYAxisA = new Vec2();
Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA);
ReferenceAngle = def.ReferenceAngle;
Impulse = new Vec3();
MotorMass = 0.0f;
MotorImpulse = 0.0f;
LowerTranslation = def.LowerTranslation;
UpperTranslation = def.UpperTranslation;
m_maxMotorForce = def.MaxMotorForce;
m_motorSpeed = def.MotorSpeed;
m_limitEnabled = def.EnableLimit;
m_motorEnabled = def.EnableMotor;
LimitState = LimitState.Inactive;
K = new Mat33();
Axis = new Vec2();
Perp = new Vec2();
}
public void WarmStart()
{
// Warm start.
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float iA = vc.InvIA;
float mB = vc.InvMassB;
float iB = vc.InvIB;
int pointCount = vc.PointCount;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
Vec2 normal = vc.Normal;
Vec2.CrossToOutUnsafe(normal, 1.0f, tangent);
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
//Console.WriteLine("vcp normal impulse is " + vcp.normalImpulse);
temp.Set(normal).MulLocal(vcp.NormalImpulse);
P.Set(tangent).MulLocal(vcp.TangentImpulse).AddLocal(temp);
wA -= iA * Vec2.Cross(vcp.RA, P);
vA.SubLocal(temp.Set(P).MulLocal(mA));
//Debug.Assert(vA.x == 0);
//Debug.Assert(wA == 0);
wB += iB * Vec2.Cross(vcp.RB, P);
vB.AddLocal(temp.Set(P).MulLocal(mB));
//Debug.Assert(vB.x == 0);
//Debug.Assert(wB == 0);
}
Velocities[indexA].W = wA;
Velocities[indexB].W = wB;
//Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " - " + wA);
//Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " - " + wB);
}
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Vec2 vpA = Pool.PopVec2();
Vec2 vpB = Pool.PopVec2();
// Cdot = dot(u, v + cross(w, r))
Vec2.CrossToOutUnsafe(wA, RA, vpA);
vpA.AddLocal(vA);
Vec2.CrossToOutUnsafe(wB, RB, vpB);
vpB.AddLocal(vB);
float Cdot = Vec2.Dot(U, vpB.SubLocal(vpA));
float impulse = (-Mass) * (Cdot + Bias + Gamma * Impulse);
Impulse += impulse;
float Px = impulse * U.X;
float Py = impulse * U.Y;
vA.X -= InvMassA * Px;
vA.Y -= InvMassA * Py;
wA -= InvIA * (RA.X * Py - RA.Y * Px);
vB.X += InvMassB * Px;
vB.Y += InvMassB * Py;
wB += InvIB * (RB.X * Py - RB.Y * Px);
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(2);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Vec2 vpA = Pool.PopVec2();
Vec2 vpB = Pool.PopVec2();
Vec2 PA = Pool.PopVec2();
Vec2 PB = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, vpA);
vpA.AddLocal(vA);
Vec2.CrossToOutUnsafe(wB, RB, vpB);
vpB.AddLocal(vB);
float Cdot = -Vec2.Dot(m_uA, vpA) - m_ratio * Vec2.Dot(m_uB, vpB);
float impulse = (-m_mass) * Cdot;
m_impulse += impulse;
PA.Set(m_uA).MulLocal(-impulse);
PB.Set(m_uB).MulLocal((-m_ratio) * impulse);
vA.X += InvMassA * PA.X;
vA.Y += InvMassA * PA.Y;
wA += InvIA * Vec2.Cross(RA, PA);
vB.X += InvMassB * PB.X;
vB.Y += InvMassB * PB.Y;
wB += InvIB * Vec2.Cross(RB, PB);
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(4);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
// Cdot = v + cross(w, r)
Vec2 Cdot = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB);
Vec2 impulse = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
temp.Set(m_impulse).MulLocal(m_gamma).AddLocal(m_C).AddLocal(Cdot).NegateLocal();
Mat22.MulToOutUnsafe(m_mass, temp, impulse);
Vec2 oldImpulse = temp;
oldImpulse.Set(m_impulse);
m_impulse.AddLocal(impulse);
float maxImpulse = data.Step.Dt * m_maxForce;
if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_impulse.MulLocal(maxImpulse / m_impulse.Length());
}
impulse.Set(m_impulse).SubLocal(oldImpulse);
vB.X += InvMassB * m_impulse.X;
vB.Y += InvMassB * m_impulse.Y;
wB += InvIB * Vec2.Cross(RB, impulse);
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(3);
}
public void InitializeVelocityConstraints()
{
//Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts");
// Warm start.
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
ContactPositionConstraint pc = PositionConstraints[i];
float radiusA = pc.RadiusA;
float radiusB = pc.RadiusB;
Manifold manifold = Contacts[vc.ContactIndex].Manifold;
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
Vec2 localCenterA = pc.LocalCenterA;
Vec2 localCenterB = pc.LocalCenterB;
Vec2 cA = Positions[indexA].C;
float aA = Positions[indexA].A;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 cB = Positions[indexB].C;
float aB = Positions[indexB].A;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
Debug.Assert(manifold.PointCount > 0);
xfA.Q.Set(aA);
xfB.Q.Set(aB);
Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp);
xfA.P.Set(cA).SubLocal(temp);
Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp);
xfB.P.Set(cB).SubLocal(temp);
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc.Normal.Set(worldManifold.Normal);
int pointCount = vc.PointCount;
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA);
vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB);
float rnA = Vec2.Cross(vcp.RA, vc.Normal);
float rnB = Vec2.Cross(vcp.RB, vc.Normal);
float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent);
float rtA = Vec2.Cross(vcp.RA, tangent);
float rtB = Vec2.Cross(vcp.RB, tangent);
float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp.VelocityBias = 0.0f;
Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1);
Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2);
temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2);
float vRel = Vec2.Dot(vc.Normal, temp);
if (vRel < -Settings.VELOCITY_THRESHOLD)
{
vcp.VelocityBias = (-vc.Restitution) * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc.PointCount == 2)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1];
float rn1A = Vec2.Cross(vcp1.RA, vc.Normal);
float rn1B = Vec2.Cross(vcp1.RB, vc.Normal);
float rn2A = Vec2.Cross(vcp2.RA, vc.Normal);
float rn2B = Vec2.Cross(vcp2.RB, vc.Normal);
float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
if (k11 * k11 < MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc.K.Ex.Set(k11, k12);
vc.K.Ey.Set(k12, k22);
vc.K.InvertToOut(vc.NormalMass);
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc.PointCount = 1;
}
}
}
}
/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso>
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
Vec2 vB = data.Velocities[m_indexB].V;
float wB = data.Velocities[m_indexB].W;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Vec2 Cdot1 = Pool.PopVec2();
Vec2 P = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
if (Frequency > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z);
m_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse1 = P;
Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
impulse1.NegateLocal();
m_impulse.X += impulse1.X;
m_impulse.Y += impulse1.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * Vec2.Cross(m_rA, P);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * Vec2.Cross(m_rB, P);
}
else
{
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mat33.MulToOutUnsafe(m_mass, Cdot, impulse);
impulse.NegateLocal();
m_impulse.AddLocal(impulse);
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z);
Pool.PushVec3(2);
}
data.Velocities[m_indexA].V.Set(vA);
data.Velocities[m_indexA].W = wA;
data.Velocities[m_indexB].V.Set(vB);
data.Velocities[m_indexB].W = wB;
Pool.PushVec2(3);
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures. This
/// normally does not need to be called unless you called setMassData to override the mass and you
/// later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
Mass = 0.0f;
InvMass = 0.0f;
I = 0.0f;
InvI = 0.0f;
Sweep.LocalCenter.SetZero();
// Static and kinematic bodies have zero mass.
if (m_type == BodyType.Static || m_type == BodyType.Kinematic)
{
// m_sweep.c0 = m_sweep.c = m_xf.position;
Sweep.C0.Set(Xf.P);
Sweep.C.Set(Xf.P);
Sweep.A0 = Sweep.A;
return;
}
Debug.Assert(m_type == BodyType.Dynamic);
// Accumulate mass over all fixtures.
Vec2 localCenter = World.Pool.PopVec2();
localCenter.SetZero();
Vec2 temp = World.Pool.PopVec2();
MassData massData = pmd;
for (Fixture f = FixtureList; f != null; f = f.Next)
{
if (f.Density == 0.0f)
{
continue;
}
f.GetMassData(massData);
Mass += massData.Mass;
// center += massData.mass * massData.center;
temp.Set(massData.Center).MulLocal(massData.Mass);
localCenter.AddLocal(temp);
I += massData.I;
}
// Compute center of mass.
if (Mass > 0.0f)
{
InvMass = 1.0f / Mass;
localCenter.MulLocal(InvMass);
}
else
{
// Force all dynamic bodies to have a positive mass.
Mass = 1.0f;
InvMass = 1.0f;
}
if (I > 0.0f && (Flags & TypeFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
I -= Mass * Vec2.Dot(localCenter, localCenter);
Debug.Assert(I > 0.0f);
InvI = 1.0f / I;
}
else
{
I = 0.0f;
InvI = 0.0f;
}
Vec2 oldCenter = World.Pool.PopVec2();
// Move center of mass.
oldCenter.Set(Sweep.C);
Sweep.LocalCenter.Set(localCenter);
// m_sweep.c0 = m_sweep.c = Mul(m_xf, m_sweep.localCenter);
Transform.MulToOutUnsafe(Xf, Sweep.LocalCenter, Sweep.C0);
Sweep.C.Set(Sweep.C0);
// Update center of mass velocity.
// m_linearVelocity += Cross(m_angularVelocity, m_sweep.c - oldCenter);
temp.Set(Sweep.C).SubLocal(oldCenter);
Vec2 temp2 = oldCenter;
Vec2.CrossToOutUnsafe(m_angularVelocity, temp, temp2);
m_linearVelocity.AddLocal(temp2);
World.Pool.PushVec2(3);
}
// TODO_ERIN might not need to return the separation
public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1)
{
ProxyA = proxyA;
ProxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
SweepA = sweepA;
SweepB = sweepB;
SweepA.GetTransform(xfa, t1);
SweepB.GetTransform(xfb, t1);
// log.debug("initializing separation.\n" +
// "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
// "distance: "+proxyA.
if (count == 1)
{
Type = Type.Points;
/*
* Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
* m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
* pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
*/
localPointA.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
Axis.Set(pointB).SubLocal(pointA);
float s = Axis.Normalize();
return(s);
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
Type = Type.FaceB;
localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0]));
localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1]));
temp.Set(localPointB2).SubLocal(localPointB1);
Vec2.CrossToOutUnsafe(temp, 1f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfb.Q, Axis, normal);
LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfb, LocalPoint, pointB);
localPointA.Set(proxyA.GetVertex(cache.IndexA[0]));
Transform.MulToOutUnsafe(xfa, localPointA, pointA);
temp.Set(pointA).SubLocal(pointB);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
Type = Type.FaceA;
localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0]));
localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1]));
temp.Set(localPointA2).SubLocal(localPointA1);
Vec2.CrossToOutUnsafe(temp, 1.0f, Axis);
Axis.Normalize();
Rot.MulToOutUnsafe(xfa.Q, Axis, normal);
LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f);
Transform.MulToOutUnsafe(xfa, LocalPoint, pointA);
localPointB.Set(ProxyB.GetVertex(cache.IndexB[0]));
Transform.MulToOutUnsafe(xfb, localPointB, pointB);
temp.Set(pointB).SubLocal(pointA);
float s = Vec2.Dot(temp, normal);
if (s < 0.0f)
{
Axis.NegateLocal();
s = -s;
}
return(s);
}
}
/// <summary>
/// Create a convex hull from the given array of points.
/// The count must be in the range [3, Settings.maxPolygonVertices].
/// This method takes an arraypool for pooling
/// </summary>
/// <warning>the points may be re-ordered, even if they form a convex polygon</warning>
/// <warning>collinear points are handled but not removed. Collinear points may lead to poor stacking behavior.</warning>
public void Set(Vec2[] verts, int num, Vec2Array vecPool, IntArray intPool)
{
Debug.Assert(3 <= num && num <= Settings.MAX_POLYGON_VERTICES);
if (num < 3)
{
SetAsBox(1.0f, 1.0f);
return;
}
int n = MathUtils.Min(num, Settings.MAX_POLYGON_VERTICES);
// Copy the vertices into a local buffer
Vec2[] ps = (vecPool != null) ? vecPool.Get(n) : new Vec2[n];
for (int i = 0; i < n; ++i)
{
ps[i] = verts[i];
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
// Find the right most point on the hull
int i0 = 0;
float x0 = ps[0].X;
for (int i = 1; i < num; ++i)
{
float x = ps[i].X;
if (x > x0 || (x == x0 && ps[i].Y < ps[i0].Y))
{
i0 = i;
x0 = x;
}
}
int[] hull = (intPool != null) ? intPool.Get(Settings.MAX_POLYGON_VERTICES) : new int[Settings.MAX_POLYGON_VERTICES];
int m = 0;
int ih = i0;
while (true)
{
hull[m] = ih;
int ie = 0;
for (int j = 1; j < n; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
Vec2 r = pool1.Set(ps[ie]).SubLocal(ps[hull[m]]);
Vec2 v = pool2.Set(ps[j]).SubLocal(ps[hull[m]]);
float c = Vec2.Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
VertexCount = m;
// Copy vertices.
for (int i = 0; i < VertexCount; ++i)
{
if (Vertices[i] == null)
{
Vertices[i] = new Vec2();
}
Vertices[i].Set(ps[hull[i]]);
}
Vec2 edge = pool1;
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < VertexCount; ++i)
{
int i1 = i;
int i2 = i + 1 < VertexCount ? i + 1 : 0;
edge.Set(Vertices[i2]).SubLocal(Vertices[i1]);
Debug.Assert(edge.LengthSquared() > Settings.EPSILON * Settings.EPSILON);
Vec2.CrossToOutUnsafe(edge, 1f, Normals[i]);
Normals[i].Normalize();
}
// Compute the polygon centroid.
ComputeCentroidToOut(Vertices, VertexCount, Centroid);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
bool fixedRotation = (iA + iB == 0.0f);
// Solve motor constraint.
if (m_motorEnabled && LimitState != LimitState.Equal && fixedRotation == false)
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = (-MotorMass) * Cdot;
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.Dt * m_maxMotorTorque;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
Vec2 temp = Pool.PopVec2();
// Solve limit constraint.
if (m_limitEnabled && LimitState != LimitState.Inactive && fixedRotation == false)
{
Vec2 Cdot1 = Pool.PopVec2();
Vec3 Cdot = Pool.PopVec3();
// Solve point-to-point constraint
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mass.Solve33ToOut(Cdot, impulse);
impulse.NegateLocal();
if (LimitState == LimitState.Equal)
{
Impulse.AddLocal(impulse);
}
else if (LimitState == LimitState.AtLower)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse < 0.0f)
{
//UPGRADE_NOTE: Final was removed from the declaration of 'rhs '. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1003'"
Vec2 rhs = Pool.PopVec2();
rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1);
Mass.Solve22ToOut(rhs, temp);
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = -Impulse.Z;
Impulse.X += temp.X;
Impulse.Y += temp.Y;
Impulse.Z = 0.0f;
Pool.PushVec2(1);
}
else
{
Impulse.AddLocal(impulse);
}
}
else if (LimitState == LimitState.AtUpper)
{
float newImpulse = Impulse.Z + impulse.Z;
if (newImpulse > 0.0f)
{
Vec2 rhs = Pool.PopVec2();
rhs.Set(Mass.Ez.X, Mass.Ez.Y).MulLocal(Impulse.Z).SubLocal(Cdot1);
Mass.Solve22ToOut(rhs, temp);
impulse.X = temp.X;
impulse.Y = temp.Y;
impulse.Z = -Impulse.Z;
Impulse.X += temp.X;
Impulse.Y += temp.Y;
Impulse.Z = 0.0f;
Pool.PushVec2(1);
}
else
{
Impulse.AddLocal(impulse);
}
}
Vec2 P = Pool.PopVec2();
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(RA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(RB, P) + impulse.Z);
Pool.PushVec2(2);
Pool.PushVec3(2);
}
else
{
// Solve point-to-point constraint
Vec2 Cdot = Pool.PopVec2();
Vec2 impulse = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Mass.Solve22ToOut(Cdot.NegateLocal(), impulse); // just leave negated
Impulse.X += impulse.X;
Impulse.Y += impulse.Y;
vA.X -= mA * impulse.X;
vA.Y -= mA * impulse.Y;
wA -= iA * Vec2.Cross(RA, impulse);
vB.X += mB * impulse.X;
vB.Y += mB * impulse.Y;
wB += iB * Vec2.Cross(RB, impulse);
Pool.PushVec2(2);
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(1);
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback to perform a exact
/// ray-cast in the case were the proxy contains a shape. The callback also performs the any
/// collision filtering. This has performance roughly equal to k * log(n), where k is the number of
/// collisions and n is the number of proxies in the tree.
/// </summary>
/// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
/// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
public void Raycast(ITreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
r.Set(p2).SubLocal(p1);
Debug.Assert(r.LengthSquared() > 0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2.CrossToOutUnsafe(1f, r, v);
absV.Set(v).AbsLocal();
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
temp.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, temp, segAABB.LowerBound);
Vec2.MaxToOut(p1, temp, segAABB.UpperBound);
intStack.Push(m_root);
while (intStack.Count > 0)
{
int nodeId = intStack.Pop();
if (nodeId == TreeNode.NULL_NODE)
{
continue;
}
TreeNode node = m_nodes[nodeId];
if (!AABB.TestOverlap(node.AABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
node.AABB.GetCenterToOut(c);
node.AABB.GetExtentsToOut(h);
temp.Set(p1).SubLocal(c);
float separation = MathUtils.Abs(Vec2.Dot(v, temp)) - Vec2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.Leaf)
{
subInput.P1.Set(input.P1);
subInput.P2.Set(input.P2);
subInput.MaxFraction = maxFraction;
float value = callback.RaycastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
t.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, t, segAABB.LowerBound);
Vec2.MaxToOut(p1, t, segAABB.UpperBound);
}
}
else
{
intStack.Push(node.Child1);
intStack.Push(node.Child2);
}
}
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
float h = data.Step.Dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = (-AngularMass) * Cdot;
float oldImpulse = m_angularImpulse;
float maxImpulse = h * m_maxTorque;
m_angularImpulse = MathUtils.Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vec2 Cdot = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2.CrossToOutUnsafe(wA, RA, temp);
Vec2.CrossToOutUnsafe(wB, RB, Cdot);
Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse = Pool.PopVec2();
Mat22.MulToOutUnsafe(LinearMass, Cdot, impulse);
impulse.NegateLocal();
Vec2 oldImpulse = Pool.PopVec2();
oldImpulse.Set(m_linearImpulse);
m_linearImpulse.AddLocal(impulse);
float maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse.MulLocal(maxImpulse);
}
impulse.Set(m_linearImpulse).SubLocal(oldImpulse);
temp.Set(impulse).MulLocal(mA);
vA.SubLocal(temp);
wA -= iA * Vec2.Cross(RA, impulse);
temp.Set(impulse).MulLocal(mB);
vB.AddLocal(temp);
wB += iB * Vec2.Cross(RB, impulse);
}
data.Velocities[IndexA].V.Set(vA);
if (data.Velocities[IndexA].W != wA)
{
Debug.Assert(data.Velocities[IndexA].W != wA);
}
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(4);
}
