public b2PrismaticJoint(b2PrismaticJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_localXAxisA = def.localAxisA;
m_localXAxisA.Normalize();
m_localYAxisA = m_localXAxisA.NegUnitCross(); // b2Math.b2Cross(1.0f, m_localXAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse.SetZero();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = b2LimitState.e_inactiveLimit;
m_axis.SetZero();
m_perp.SetZero();
}
public b2Vec2 GetSearchDirection()
{
switch (m_count)
{
case 1:
return(-m_vertices[0].w);
case 2:
{
b2Vec2 e12 = m_vertices[1].w - m_vertices[0].w;
float sgn = b2Math.b2Cross(e12, -m_vertices[0].w);
if (sgn > 0.0f)
{
// Origin is left of e12.
return(e12.NegUnitCross()); // b2Math.b2Cross(1.0f, e12);
}
else
{
// Origin is right of e12.
return(e12.UnitCross()); // b2Math.b2Cross(e12, 1.0f);
}
}
default:
Debug.Assert(false);
return(b2Vec2.Zero);
}
}
public b2WheelJoint(b2WheelJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_localXAxisA = def.localAxisA;
m_localYAxisA = m_localXAxisA.NegUnitCross();
// m_localYAxisA = b2Math.b2Cross(1.0f, m_localXAxisA);
m_mass = 0.0f;
m_impulse = 0.0f;
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_springMass = 0.0f;
m_springImpulse = 0.0f;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableMotor = def.enableMotor;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_bias = 0.0f;
m_gamma = 0.0f;
m_ax.SetZero();
m_ay.SetZero();
}
public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = r.NegUnitCross(); // b2Math.b2Cross(1.0f, r);
b2Vec2 abs_v = b2Math.b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB = b2AABB.Default;
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
Stack <int> stack = new Stack <int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
b2Vec2 c = node.aabb.Center;
b2Vec2 h = node.aabb.Extents;
float separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = 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;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}