public static void mulTransToOutUnsafe(Transform A, Transform B,
Transform out_)
{
Rot.mulTransUnsafe(A.q, B.q, out_.q);
pool.set(B.p).subLocal(A.p);
Rot.mulTransUnsafe(A.q, pool, out_.p);
}
public double evaluate(int indexA, int indexB, double t)
{
m_sweepA.getTransform(xfa, t);
m_sweepB.getTransform(xfb, t);
switch (m_type)
{
case Type.POINTS:
{
Rot.mulTransUnsafe(xfa.q, m_axis, axisA);
Rot.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB);
m_axis.negateLocal();
localPointA.set(m_proxyA.getVertex(indexA));
localPointB.set(m_proxyB.getVertex(indexB));
Transform.mulToOutUnsafe(xfa, localPointA, pointA);
Transform.mulToOutUnsafe(xfb, localPointB, pointB);
double separation = Vec2.dot(pointB.subLocal(pointA), m_axis);
return(separation);
}
case Type.FACE_A:
{
// System.out.printf("We're faceA");
Rot.mulToOutUnsafe(xfa.q, m_axis, normal);
Transform.mulToOutUnsafe(xfa, m_localPoint, pointA);
Rot.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB);
normal.negateLocal();
localPointB.set(m_proxyB.getVertex(indexB));
Transform.mulToOutUnsafe(xfb, localPointB, pointB);
double separation = Vec2.dot(pointB.subLocal(pointA), normal);
return(separation);
}
case Type.FACE_B:
{
// System.out.printf("We're faceB");
Rot.mulToOutUnsafe(xfb.q, m_axis, normal);
Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);
Rot.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA);
normal.negateLocal();
localPointA.set(m_proxyA.getVertex(indexA));
Transform.mulToOutUnsafe(xfa, localPointA, pointA);
double separation = Vec2.dot(pointA.subLocal(pointB), normal);
return(separation);
}
default:
return(0d);
}
}
public static Transform mulTrans(Transform A, Transform B)
{
var C = new Transform();
Rot.mulTransUnsafe(A.q, B.q, C.q);
pool.set(B.p).subLocal(A.p);
Rot.mulTransUnsafe(A.q, pool, C.p);
return(C);
}
public void getLocalVectorToOutUnsafe(Vec2 worldVector, Vec2 out_)
{
Rot.mulTransUnsafe(m_xf.q, worldVector, out_);
}
// double FindMinSeparation(int* indexA, int* indexB, double t) const
public double findMinSeparation(int[] indexes, double t)
{
m_sweepA.getTransform(xfa, t);
m_sweepB.getTransform(xfb, t);
switch (m_type)
{
case Type.POINTS:
{
Rot.mulTransUnsafe(xfa.q, m_axis, axisA);
Rot.mulTransUnsafe(xfb.q, m_axis.negateLocal(), axisB);
m_axis.negateLocal();
indexes[0] = m_proxyA.getSupport(axisA);
indexes[1] = m_proxyB.getSupport(axisB);
localPointA.set(m_proxyA.getVertex(indexes[0]));
localPointB.set(m_proxyB.getVertex(indexes[1]));
Transform.mulToOutUnsafe(xfa, localPointA, pointA);
Transform.mulToOutUnsafe(xfb, localPointB, pointB);
double separation = Vec2.dot(pointB.subLocal(pointA), m_axis);
return(separation);
}
case Type.FACE_A:
{
Rot.mulToOutUnsafe(xfa.q, m_axis, normal);
Transform.mulToOutUnsafe(xfa, m_localPoint, pointA);
Rot.mulTransUnsafe(xfb.q, normal.negateLocal(), axisB);
normal.negateLocal();
indexes[0] = -1;
indexes[1] = m_proxyB.getSupport(axisB);
localPointB.set(m_proxyB.getVertex(indexes[1]));
Transform.mulToOutUnsafe(xfb, localPointB, pointB);
double separation = Vec2.dot(pointB.subLocal(pointA), normal);
return(separation);
}
case Type.FACE_B:
{
Rot.mulToOutUnsafe(xfb.q, m_axis, normal);
Transform.mulToOutUnsafe(xfb, m_localPoint, pointB);
Rot.mulTransUnsafe(xfa.q, normal.negateLocal(), axisA);
normal.negateLocal();
indexes[1] = -1;
indexes[0] = m_proxyA.getSupport(axisA);
localPointA.set(m_proxyA.getVertex(indexes[0]));
Transform.mulToOutUnsafe(xfa, localPointA, pointA);
double separation = Vec2.dot(pointA.subLocal(pointB), normal);
return(separation);
}
default:
indexes[0] = -1;
indexes[1] = -1;
return(0d);
}
}
/**
* Compute the closest points between two shapes. Supports any combination of: CircleShape and
* PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
* zero.
*
* @param output
* @param cache
* @param input
*/
public void distance(DistanceOutput output, SimplexCache cache,
DistanceInput input)
{
GJK_CALLS++;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Transform transformA = input.transformA;
Transform transformB = input.transformB;
// Initialize the simplex.
simplex.readCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
int saveCount = 0;
simplex.getClosestPoint(closestPoint);
double distanceSqr1 = closestPoint.lengthSquared();
double distanceSqr2 = distanceSqr1;
// Main iteration loop
int iter = 0;
while (iter < GJK_MAX_ITERS)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.solve2();
break;
case 3:
simplex.solve3();
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute closest point.
simplex.getClosestPoint(closestPoint);
distanceSqr2 = closestPoint.lengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1)
{
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.getSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.lengthSquared() < Settings.EPSILON * Settings.EPSILON)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.m_count];
Rot.mulTransUnsafe(transformA.q, d.negateLocal(), temp);
vertex.indexA = proxyA.getSupport(temp);
Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), vertex.wA);
// Vec2 wBLocal;
Rot.mulTransUnsafe(transformB.q, d.negateLocal(), temp);
vertex.indexB = proxyB.getSupport(temp);
Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), vertex.wB);
vertex.w.set(vertex.wB).subLocal(vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.getWitnessPoints(output.pointA, output.pointB);
output.distance = MathUtils.distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.writeCache(cache);
// Apply radii if requested.
if (input.useRadii)
{
double rA = proxyA.m_radius;
double rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > Settings.EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the out_er surface.
output.distance -= rA + rB;
normal.set(output.pointB).subLocal(output.pointA);
normal.normalize();
temp.set(normal).mulLocal(rA);
output.pointA.addLocal(temp);
temp.set(normal).mulLocal(rB);
output.pointB.subLocal(temp);
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5d * (output.pointA + output.pointB);
output.pointA.addLocal(output.pointB).mulLocal(.5d);
output.pointB.set(output.pointA);
output.distance = 0.0d;
}
}
}
public override bool solvePositionConstraints(SolverData data)
{
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Vec2 cC = data.positions[m_indexC].c;
double aC = data.positions[m_indexC].a;
Vec2 cD = data.positions[m_indexD].c;
double aD = data.positions[m_indexD].a;
Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot();
qA.set(aA);
qB.set(aB);
qC.set(aC);
qD.set(aD);
double linearError = 0.0d;
double coordinateA, coordinateB;
Vec2 temp = pool.popVec2();
Vec2 JvAC = pool.popVec2();
Vec2 JvBD = pool.popVec2();
double JwA, JwB, JwC, JwD;
double mass = 0.0d;
if (m_typeA == JointType.REVOLUTE)
{
JvAC.setZero();
JwA = 1.0d;
JwC = 1.0d;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
Vec2 rC = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 pC = pool.popVec2();
Vec2 pA = pool.popVec2();
Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC);
Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC);
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA);
JwC = Vec2.cross(rC, JvAC);
JwA = Vec2.cross(rA, JvAC);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
pC.set(m_localAnchorC).subLocal(m_lcC);
Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA);
coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC);
pool.pushVec2(4);
}
if (m_typeB == JointType.REVOLUTE)
{
JvBD.setZero();
JwB = m_ratio;
JwD = m_ratio;
mass += m_ratio * m_ratio * (m_iB + m_iD);
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
Vec2 u = pool.popVec2();
Vec2 rD = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 pD = pool.popVec2();
Vec2 pB = pool.popVec2();
Rot.mulToOutUnsafe(qD, m_localAxisD, u);
Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB);
JvBD.set(u).mulLocal(m_ratio);
JwD = Vec2.cross(rD, u);
JwB = Vec2.cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
pD.set(m_localAnchorD).subLocal(m_lcD);
Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB);
coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD);
pool.pushVec2(5);
}
double C = (coordinateA + m_ratio * coordinateB) - m_constant;
double impulse = 0.0d;
if (mass > 0.0d)
{
impulse = -C / mass;
}
pool.pushVec2(3);
pool.pushRot(4);
cA.x += (m_mA * impulse) * JvAC.x;
cA.y += (m_mA * impulse) * JvAC.y;
aA += m_iA * impulse * JwA;
cB.x += (m_mB * impulse) * JvBD.x;
cB.y += (m_mB * impulse) * JvBD.y;
aB += m_iB * impulse * JwB;
cC.x -= (m_mC * impulse) * JvAC.x;
cC.y -= (m_mC * impulse) * JvAC.y;
aC -= m_iC * impulse * JwC;
cD.x -= (m_mD * impulse) * JvBD.x;
cD.y -= (m_mD * impulse) * JvBD.y;
aD -= m_iD * impulse * JwD;
// data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
// data.positions[m_indexC].c = cC;
data.positions[m_indexC].a = aC;
// data.positions[m_indexD].c = cD;
data.positions[m_indexD].a = aD;
// TODO_ERIN not implemented
return(linearError < Settings.linearSlop);
}
public GearJoint(IWorldPool argWorldPool, GearJointDef def)
: base(argWorldPool, def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.getType();
m_typeB = m_joint2.getType();
double coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in Joint.
m_bodyC = m_joint1.getBodyA();
m_bodyA = m_joint1.getBodyB();
// Get geometry of joint1
Transform xfA = m_bodyA.m_xf;
double aA = m_bodyA.m_sweep.a;
Transform xfC = m_bodyC.m_xf;
double aC = m_bodyC.m_sweep.a;
if (m_typeA == JointType.REVOLUTE)
{
var revolute = (RevoluteJoint)def.joint1;
m_localAnchorC.set(revolute.m_localAnchorA);
m_localAnchorA.set(revolute.m_localAnchorB);
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.setZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
Vec2 pA = pool.popVec2();
Vec2 temp = pool.popVec2();
var prismatic = (PrismaticJoint)def.joint1;
m_localAnchorC.set(prismatic.m_localAnchorA);
m_localAnchorA.set(prismatic.m_localAnchorB);
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC.set(prismatic.m_localXAxisA);
Vec2 pC = m_localAnchorC;
Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, temp);
temp.addLocal(xfA.p).subLocal(xfC.p);
Rot.mulTransUnsafe(xfC.q, temp, pA);
coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC);
pool.pushVec2(2);
}
m_bodyD = m_joint2.getBodyA();
m_bodyB = m_joint2.getBodyB();
// Get geometry of joint2
Transform xfB = m_bodyB.m_xf;
double aB = m_bodyB.m_sweep.a;
Transform xfD = m_bodyD.m_xf;
double aD = m_bodyD.m_sweep.a;
if (m_typeB == JointType.REVOLUTE)
{
var revolute = (RevoluteJoint)def.joint2;
m_localAnchorD.set(revolute.m_localAnchorA);
m_localAnchorB.set(revolute.m_localAnchorB);
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.setZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
Vec2 pB = pool.popVec2();
Vec2 temp = pool.popVec2();
var prismatic = (PrismaticJoint)def.joint2;
m_localAnchorD.set(prismatic.m_localAnchorA);
m_localAnchorB.set(prismatic.m_localAnchorB);
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD.set(prismatic.m_localXAxisA);
Vec2 pD = m_localAnchorD;
Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, temp);
temp.addLocal(xfB.p).subLocal(xfD.p);
Rot.mulTransUnsafe(xfD.q, temp, pB);
coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD);
pool.pushVec2(2);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0d;
}
