public override void getReactionForce(float inv_dt, Vec2 argOut)
{
Vec2 temp = pool.popVec2();
temp.set_Renamed(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
argOut.set_Renamed(m_perp).mulLocal(m_impulse.x).addLocal(temp).mulLocal(inv_dt);
pool.pushVec2(1);
}
public void getLinearVelocityFromWorldPointToOut(Vec2 worldPoint, Vec2 out_Renamed)
{
out_Renamed.set_Renamed(worldPoint).subLocal(m_sweep.c);
Vec2.crossToOut(m_angularVelocity, out_Renamed, out_Renamed);
out_Renamed.addLocal(m_linearVelocity);
}
public override void getReactionForce(float inv_dt, Vec2 argOut)
{
argOut.set_Renamed(m_uB).mulLocal(m_impulse).mulLocal(inv_dt);
}
public virtual void getXAxis(Vec2 xAxis)
{
xAxis.set_Renamed(c, s);
}
public virtual void getYAxis(Vec2 yAxis)
{
yAxis.set_Renamed(-s, c);
}
public virtual void getWitnessPoints(Vec2 pA, Vec2 pB)
{
switch (m_count)
{
case 0:
Debug.Assert(false);
break;
case 1:
pA.set_Renamed(m_v1.wA);
pB.set_Renamed(m_v1.wB);
break;
case 2:
case2.set_Renamed(m_v1.wA).mulLocal(m_v1.a);
pA.set_Renamed(m_v2.wA).mulLocal(m_v2.a).addLocal(case2);
// m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
// *pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
case2.set_Renamed(m_v1.wB).mulLocal(m_v1.a);
pB.set_Renamed(m_v2.wB).mulLocal(m_v2.a).addLocal(case2);
break;
case 3:
pA.set_Renamed(m_v1.wA).mulLocal(m_v1.a);
case3.set_Renamed(m_v2.wA).mulLocal(m_v2.a);
case33.set_Renamed(m_v3.wA).mulLocal(m_v3.a);
pA.addLocal(case3).addLocal(case33);
pB.set_Renamed(pA);
// *pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
// *pB = *pA;
break;
default:
Debug.Assert(false);
break;
}
}
public override void getAnchorA(Vec2 argOut)
{
argOut.set_Renamed(m_targetA);
}
public void getSearchDirection(Vec2 out_Renamed)
{
switch (m_count)
{
case 1:
out_Renamed.set_Renamed(m_v1.w).negateLocal();
return;
case 2:
e12.set_Renamed(m_v2.w).subLocal(m_v1.w);
// use out for a temp variable real quick
out_Renamed.set_Renamed(m_v1.w).negateLocal();
float sgn = Vec2.cross(e12, out_Renamed);
if (sgn > 0f)
{
// Origin is left of e12.
Vec2.crossToOutUnsafe(1f, e12, out_Renamed);
return;
}
else
{
// Origin is right of e12.
Vec2.crossToOutUnsafe(e12, 1f, out_Renamed);
return;
}
goto default;
default:
Debug.Assert(false);
out_Renamed.setZero();
return;
}
}
/// <summary> this returns pooled objects. don't keep or modify them
///
/// </summary>
/// <returns>
/// </returns>
public virtual void getClosestPoint(Vec2 out_Renamed)
{
switch (m_count)
{
case 0:
Debug.Assert(false);
out_Renamed.setZero();
return;
case 1:
out_Renamed.set_Renamed(m_v1.w);
return;
case 2:
case22.set_Renamed(m_v2.w).mulLocal(m_v2.a);
case2.set_Renamed(m_v1.w).mulLocal(m_v1.a).addLocal(case22);
out_Renamed.set_Renamed(case2);
return;
case 3:
out_Renamed.setZero();
return;
default:
Debug.Assert(false);
out_Renamed.setZero();
return;
}
}
public void computeCentroidToOut(Vec2[] vs, int count, Vec2 out_Renamed)
{
Debug.Assert(count >= 3);
out_Renamed.set_Renamed(0.0f, 0.0f);
float area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = pool1;
pRef.setZero();
Vec2 e1 = pool2;
Vec2 e2 = pool3;
float inv3 = 1.0f / 3.0f;
for (int i = 0; i < count; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = vs[i];
Vec2 p3 = i + 1 < count ? vs[i + 1] : vs[0];
e1.set_Renamed(p2).subLocal(p1);
e2.set_Renamed(p3).subLocal(p1);
float D = Vec2.cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
e1.set_Renamed(p1).addLocal(p2).addLocal(p3).mulLocal(triangleArea * inv3);
out_Renamed.addLocal(e1);
}
// Centroid
Debug.Assert(area > Settings.EPSILON);
out_Renamed.mulLocal(1.0f / area);
}
/// <summary>
/// Takes the world coordinates and puts the corresponding screen
/// coordinates in argScreen.
/// </summary>
/// <param name="worldX"></param>
/// <param name="worldY"></param>
/// <param name="argScreen"></param>
public virtual void getWorldToScreenToOut(float worldX, float worldY, Vec2 argScreen)
{
argScreen.set_Renamed(worldX, worldY);
viewportTransform.getWorldToScreen(argScreen, argScreen);
}
/// <summary>
/// takes the screen coordinates and puts the corresponding
/// world coordinates in argWorld.
/// </summary>
/// <param name="screenX"></param>
/// <param name="screenY"></param>
/// <param name="argWorld"></param>
public virtual void getScreenToWorldToOut(float screenX, float screenY, Vec2 argWorld)
{
argWorld.set_Renamed(screenX, screenY);
viewportTransform.getScreenToWorld(argWorld, argWorld);
}
