// Cache here per time step to reduce cache misses.
// Vec2 m_localCenterA, m_localCenterB;
// double m_invMassA, m_invIA;
// double m_invMassB, m_invIB;
public Joint(IWorldPool worldPool, JointDef def)
{
pool = worldPool;
m_type = def.type;
m_prev = null;
m_next = null;
m_bodyA = def.bodyA;
m_bodyB = def.bodyB;
m_collideConnected = def.collideConnected;
m_islandFlag = false;
m_userData = def.userData;
m_edgeA = new JointEdge();
m_edgeA.joint = null;
m_edgeA.other = null;
m_edgeA.prev = null;
m_edgeA.next = null;
m_edgeB = new JointEdge();
m_edgeB.joint = null;
m_edgeB.other = null;
m_edgeB.prev = null;
m_edgeB.next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1d, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
m_motorMass = 0.0d;
m_motorImpulse = 0.0d;
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 = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
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();
}
// Cache here per time step to reduce cache misses.
// Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected Joint(IWorldPool worldPool, JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
pool = worldPool;
m_type = def.type;
m_prev = null;
m_next = null;
m_bodyA = def.bodyA;
m_bodyB = def.bodyB;
m_collideConnected = def.collideConnected;
m_islandFlag = false;
m_userData = def.userData;
m_edgeA = new JointEdge();
m_edgeA.joint = null;
m_edgeA.other = null;
m_edgeA.prev = null;
m_edgeA.next = null;
m_edgeB = new JointEdge();
m_edgeB.joint = null;
m_edgeB.other = null;
m_edgeB.prev = null;
m_edgeB.next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
// Cache here per time step to reduce cache misses.
// final Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected internal Joint(IWorldPool argWorldPool, JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
Pool = argWorldPool;
Type = def.Type;
Prev = null;
Next = null;
BodyA = def.BodyA;
BodyB = def.BodyB;
CollideConnected = def.CollideConnected;
IslandFlag = false;
UserData = def.UserData;
Index = 0;
EdgeA = new JointEdge();
EdgeA.Joint = null;
EdgeA.Other = null;
EdgeA.Prev = null;
EdgeA.Next = null;
EdgeB = new JointEdge();
EdgeB.Joint = null;
EdgeB.Other = null;
EdgeB.Prev = null;
EdgeB.Next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
public Contact(IWorldPool argPool)
{
m_fixtureA = null;
m_fixtureB = null;
m_nodeA = new ContactEdge();
m_nodeB = new ContactEdge();
m_manifold = new Manifold();
pool = argPool;
}
protected internal Contact(IWorldPool argPool)
{
FixtureA = null;
FixtureB = null;
NodeA = new ContactEdge();
NodeB = new ContactEdge();
Manifold = new Manifold();
Pool = argPool;
}
protected internal Contact(IWorldPool argPool)
{
FixtureA = null;
FixtureB = null;
NodeA = new ContactEdge();
NodeB = new ContactEdge();
Manifold = new Manifold();
Pool = argPool;
}
public Collision(IWorldPool argPool)
{
incidentEdge[0] = new ClipVertex();
incidentEdge[1] = new ClipVertex();
clipPoints1[0] = new ClipVertex();
clipPoints1[1] = new ClipVertex();
clipPoints2[0] = new ClipVertex();
clipPoints2[1] = new ClipVertex();
pool = argPool;
}
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def) : base(argWorld, def)
{
m_localAnchorA = def.localAnchorA.clone();
m_localAnchorB = def.localAnchorB.clone();
m_length = def.length;
m_impulse = 0.0d;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_gamma = 0.0d;
m_bias = 0.0d;
}
/// <param name="argWorld"></param>
/// <param name="def"></param>
protected internal WeldJoint(IWorldPool argWorld, WeldJointDef def)
: base(argWorld, def)
{
LocalAnchorA = new Vec2(def.LocalAnchorA);
LocalAnchorB = new Vec2(def.LocalAnchorB);
m_referenceAngle = def.ReferenceAngle;
Frequency = def.FrequencyHz;
DampingRatio = def.DampingRatio;
m_impulse = new Vec3();
m_impulse.SetZero();
}
/// <param name="argWorldPool"></param>
/// <param name="def"></param>
public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def)
: base(argWorldPool, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_linearImpulse = new Vec2();
m_angularImpulse = 0.0f;
m_maxForce = def.maxForce;
m_maxTorque = def.maxTorque;
}
public FrictionJoint(IWorldPool argWorldPool, FrictionJointDef def)
: base(argWorldPool, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_linearImpulse = new Vec2();
m_angularImpulse = 0.0d;
m_maxForce = def.maxForce;
m_maxTorque = def.maxTorque;
}
public WeldJoint(IWorldPool argWorld, WeldJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_impulse = new Vec3();
m_impulse.setZero();
}
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def)
: base(argWorld, def)
{
LocalAnchorA = def.LocalAnchorA.Clone();
LocalAnchorB = def.LocalAnchorB.Clone();
Length = def.Length;
Impulse = 0.0f;
FrequencyHz = def.FrequencyHz;
DampingRatio = def.DampingRatio;
Gamma = 0.0f;
Bias = 0.0f;
}
public MotorJoint(IWorldPool pool, MotorJointDef def)
: base(pool, def)
{
m_linearOffset.set(def.linearOffset);
m_angularOffset = def.angularOffset;
m_angularImpulse = 0.0f;
m_maxForce = def.maxForce;
m_maxTorque = def.maxTorque;
m_correctionFactor = def.correctionFactor;
}
public RopeJoint(IWorldPool worldPool, RopeJointDef def)
: base(worldPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_maxLength = def.maxLength;
m_mass = 0.0d;
m_impulse = 0.0d;
m_state = LimitState.INACTIVE;
m_length = 0.0d;
}
internal RopeJoint(IWorldPool worldPool, RopeJointDef def)
: base(worldPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_maxLength = def.maxLength;
m_mass = 0.0f;
m_impulse = 0.0f;
m_state = LimitState.INACTIVE;
m_length = 0.0f;
}
public MouseJoint(IWorldPool argWorld, MouseJointDef def)
: base(argWorld, def)
{
m_targetA.set(def.target);
Transform.mulTransToOutUnsafe(m_bodyB.getTransform(), m_targetA, m_localAnchorB);
m_maxForce = def.maxForce;
m_impulse.setZero();
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_beta = 0;
m_gamma = 0;
}
public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def)
: base(argWorldPool, def)
{
m_groundAnchorA.set(def.groundAnchorA);
m_groundAnchorB.set(def.groundAnchorB);
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_ratio = def.ratio;
m_lengthA = def.lengthA;
m_lengthB = def.lengthB;
m_constant = def.lengthA + m_ratio * def.lengthB;
m_impulse = 0.0d;
}
/// <param name="argWorldPool"></param>
/// <param name="def"></param>
public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def)
: base(argWorldPool, def)
{
m_groundAnchorA.Set(def.GroundAnchorA);
m_groundAnchorB.Set(def.GroundAnchorB);
LocalAnchorA.Set(def.LocalAnchorA);
LocalAnchorB.Set(def.LocalAnchorB);
Debug.Assert(def.Ratio != 0.0f);
m_ratio = def.Ratio;
m_lengthA = def.LengthA;
m_LengthB = def.LengthB;
m_constant = def.LengthA + m_ratio * def.LengthB;
m_impulse = 0.0f;
}
public RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
LocalAnchorA.Set(def.LocalAnchorA);
LocalAnchorB.Set(def.LocalAnchorB);
ReferenceAngle = def.ReferenceAngle;
MotorImpulse = 0;
LowerAngle = def.LowerAngle;
UpperAngle = def.UpperAngle;
m_maxMotorTorque = def.MaxMotorTorque;
m_motorSpeed = def.MotorSpeed;
m_limitEnabled = def.EnableLimit;
m_motorEnabled = def.EnableMotor;
LimitState = LimitState.Inactive;
}
/// <param name="argWorldPool"></param>
/// <param name="def"></param>
public PulleyJoint(IWorldPool argWorldPool, PulleyJointDef def)
: base(argWorldPool, def)
{
m_groundAnchorA.set_Renamed(def.groundAnchorA);
m_groundAnchorB.set_Renamed(def.groundAnchorB);
m_localAnchorA.set_Renamed(def.localAnchorA);
m_localAnchorB.set_Renamed(def.localAnchorB);
Debug.Assert(def.ratio != 0.0f);
m_ratio = def.ratio;
m_lengthA = def.lengthA;
m_LengthB = def.lengthB;
m_constant = def.lengthA + m_ratio * def.lengthB;
m_impulse = 0.0f;
}
public RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_motorImpulse = 0;
m_lowerAngle = def.lowerAngle;
m_upperAngle = def.upperAngle;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
}
public WheelJoint(IWorldPool argPool, WheelJointDef def) : base(argPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_localXAxisA.set(def.localAxisA);
Vec2.crossToOutUnsafe(1.0d, m_localXAxisA, m_localYAxisA);
m_motorMass = 0.0d;
m_motorImpulse = 0.0d;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableMotor = def.enableMotor;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
}
protected internal MouseJoint(IWorldPool argWorld, MouseJointDef def)
: base(argWorld, def)
{
Debug.Assert(def.Target.Valid);
Debug.Assert(def.MaxForce >= 0);
Debug.Assert(def.FrequencyHz >= 0);
Debug.Assert(def.DampingRatio >= 0);
m_targetA.Set(def.Target);
Transform.MulTransToOutUnsafe(BodyB.GetTransform(), m_targetA, m_localAnchorB);
m_maxForce = def.MaxForce;
m_impulse.SetZero();
Frequency = def.FrequencyHz;
DampingRatio = def.DampingRatio;
m_beta = 0;
m_gamma = 0;
}
protected internal MouseJoint(IWorldPool argWorld, MouseJointDef def)
: base(argWorld, def)
{
Debug.Assert(def.Target.Valid);
Debug.Assert(def.MaxForce >= 0);
Debug.Assert(def.FrequencyHz >= 0);
Debug.Assert(def.DampingRatio >= 0);
m_targetA.Set(def.Target);
Transform.MulTransToOutUnsafe(BodyB.GetTransform(), m_targetA, m_localAnchorB);
m_maxForce = def.MaxForce;
m_impulse.SetZero();
Frequency = def.FrequencyHz;
DampingRatio = def.DampingRatio;
m_beta = 0;
m_gamma = 0;
}
protected internal MouseJoint(IWorldPool argWorld, MouseJointDef def)
: base(argWorld, def)
{
Debug.Assert(def.target.Valid);
Debug.Assert(def.maxForce >= 0);
Debug.Assert(def.frequencyHz >= 0);
Debug.Assert(def.dampingRatio >= 0);
m_targetA.set_Renamed(def.target);
Transform.mulTransToOutUnsafe(m_bodyB.getTransform(), m_targetA, m_localAnchorB);
m_maxForce = def.maxForce;
m_impulse.setZero();
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_beta = 0;
m_gamma = 0;
}
internal RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_motorImpulse = 0;
m_lowerAngle = def.lowerAngle;
m_upperAngle = def.upperAngle;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="argPool"> </param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
Pool = argPool;
DestructionListener = null;
DebugDraw = null;
BodyList = null;
JointList = null;
BodyCount = 0;
JointCount = 0;
WarmStarting = true;
ContinuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
SleepingAllowed = true;
m_gravity.Set(gravity);
Flags = CLEAR_FORCES;
invDt0 = 0f;
ContactManager = new ContactManager(this);
Profile = new Profile();
InitializeRegisters();
}
public DistanceJoint(IWorldPool argWorld, DistanceJointDef def)
: base(argWorld, def)
{
m_localAnchorA = def.localAnchorA.Clone();
m_localAnchorB = def.localAnchorB.Clone();
m_length = def.length;
m_impulse = 0.0f;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_gamma = 0.0f;
m_bias = 0.0f;
}
protected internal Contact(IWorldPool argPool)
{
m_fixtureA = null;
m_fixtureB = null;
m_nodeA = new ContactEdge();
m_nodeB = new ContactEdge();
m_manifold = new Manifold();
pool = argPool;
}
public ChainAndCircleContact(IWorldPool argPool) :
base(argPool)
{
}
/// <summary>
/// From Real-time Collision Detection, p179.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
/// <param name="argPool"></param>
public bool Raycast(RayCastOutput output, RayCastInput input, IWorldPool argPool)
{
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float tmin = Single.Epsilon;
float tmax = Single.MaxValue;
Vec2 p = argPool.PopVec2();
Vec2 d = argPool.PopVec2();
Vec2 absD = argPool.PopVec2();
Vec2 normal = argPool.PopVec2();
p.Set(input.P1);
d.Set(input.P2).SubLocal(input.P1);
Vec2.AbsToOut(d, absD);
// x then y
if (absD.X < Settings.EPSILON)
{
// Parallel.
if (p.X < LowerBound.X || UpperBound.X < p.X)
{
argPool.PushVec2(4);
return(false);
}
}
else
{
float inv_d = 1.0f / d.X;
float t1 = (LowerBound.X - p.X) * inv_d;
float t2 = (UpperBound.X - p.X) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.X = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.Min(tmax, t2);
if (tmin > tmax)
{
argPool.PushVec2(4);
return(false);
}
}
if (absD.Y < Settings.EPSILON)
{
// Parallel.
if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
{
argPool.PushVec2(4);
return(false);
}
}
else
{
float inv_d = 1.0f / d.Y;
float t1 = (LowerBound.Y - p.Y) * inv_d;
float t2 = (UpperBound.Y - p.Y) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.Y = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.Min(tmax, t2);
if (tmin > tmax)
{
argPool.PushVec2(4);
return(false);
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.MaxFraction < tmin)
{
argPool.PushVec2(4);
return(false);
}
// Intersection.
output.Fraction = tmin;
output.Normal.X = normal.X;
output.Normal.Y = normal.Y;
argPool.PushVec2(4);
return(true);
}
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;
}
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
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 = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
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();
}
internal 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();
Debug.Assert(m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC);
Debug.Assert(m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC);
float 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;
float aA = m_bodyA.m_sweep.a;
Transform xfC = m_bodyC.m_xf;
float aC = m_bodyC.m_sweep.a;
if (m_typeA == JointType.REVOLUTE)
{
RevoluteJoint 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();
PrismaticJoint 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, ref temp);
temp.addLocal(xfA.p);
temp.subLocal(xfC.p);
Rot.mulTransUnsafe(xfC.q, temp, ref pA);
pA.subLocal(pC);
coordinateA = Vec2.dot(pA, 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;
float aB = m_bodyB.m_sweep.a;
Transform xfD = m_bodyD.m_xf;
float aD = m_bodyD.m_sweep.a;
if (m_typeB == JointType.REVOLUTE)
{
RevoluteJoint 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();
PrismaticJoint 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, ref temp);
temp.addLocal(xfB.p);
temp.subLocal(xfD.p);
Rot.mulTransUnsafe(xfD.q, temp, ref pB);
pB.subLocal(pD);
coordinateB = Vec2.dot(pB, m_localAxisD);
pool.pushVec2(2);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio*coordinateB;
m_impulse = 0.0f;
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="doSleep">improve performance by not simulating inactive bodies.</param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
pool = argPool;
m_destructionListener = null;
m_debugDraw = null;
m_bodyList = null;
m_jointList = null;
m_bodyCount = 0;
m_jointCount = 0;
m_warmStarting = true;
m_continuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
m_allowSleep = true;
m_gravity.set_Renamed(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this);
m_profile = new Profile();
initializeRegisters();
}
/**
* From Real-time Collision Detection, p179.
*
* @param output
* @param input
*/
public bool raycast(RayCastOutput output, RayCastInput input,
IWorldPool argPool)
{
double tmin = -double.MaxValue;
double tmax = double.MaxValue;
Vec2 p = argPool.popVec2();
Vec2 d = argPool.popVec2();
Vec2 absD = argPool.popVec2();
Vec2 normal = argPool.popVec2();
p.set(input.p1);
d.set(input.p2).subLocal(input.p1);
Vec2.absToOut(d, absD);
// x then y
if (absD.x < Settings.EPSILON)
{
// Parallel.
if (p.x < lowerBound.x || upperBound.x < p.x)
{
argPool.pushVec2(4);
return(false);
}
}
else
{
double inv_d = 1.0d / d.x;
double t1 = (lowerBound.x - p.x) * inv_d;
double t2 = (upperBound.x - p.x) * inv_d;
// Sign of the normal vector.
double s = -1.0d;
if (t1 > t2)
{
double temp = t1;
t1 = t2;
t2 = temp;
s = 1.0d;
}
// Push the min up
if (t1 > tmin)
{
normal.setZero();
normal.x = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.min(tmax, t2);
if (tmin > tmax)
{
argPool.pushVec2(4);
return(false);
}
}
if (absD.y < Settings.EPSILON)
{
// Parallel.
if (p.y < lowerBound.y || upperBound.y < p.y)
{
argPool.pushVec2(4);
return(false);
}
}
else
{
double inv_d = 1.0d / d.y;
double t1 = (lowerBound.y - p.y) * inv_d;
double t2 = (upperBound.y - p.y) * inv_d;
// Sign of the normal vector.
double s = -1.0d;
if (t1 > t2)
{
double temp = t1;
t1 = t2;
t2 = temp;
s = 1.0d;
}
// Push the min up
if (t1 > tmin)
{
normal.setZero();
normal.y = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.min(tmax, t2);
if (tmin > tmax)
{
argPool.pushVec2(4);
return(false);
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0d || input.maxFraction < tmin)
{
argPool.pushVec2(4);
return(false);
}
// Intersection.
output.fraction = tmin;
output.normal.x = normal.x;
output.normal.y = normal.y;
argPool.pushVec2(4);
return(true);
}
public PolygonContact(IWorldPool argPool)
: base(argPool)
{
}
// Cache here per time step to reduce cache misses.
// final Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected internal Joint(IWorldPool argWorldPool, JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
Pool = argWorldPool;
Type = def.Type;
Prev = null;
Next = null;
BodyA = def.BodyA;
BodyB = def.BodyB;
CollideConnected = def.CollideConnected;
IslandFlag = false;
UserData = def.UserData;
Index = 0;
EdgeA = new JointEdge();
EdgeA.Joint = null;
EdgeA.Other = null;
EdgeA.Prev = null;
EdgeA.Next = null;
EdgeB = new JointEdge();
EdgeB.Joint = null;
EdgeB.Other = null;
EdgeB.Prev = null;
EdgeB.Next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
/// <summary>
/// From Real-time Collision Detection, p179.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
public bool raycast(RayCastOutput output, RayCastInput input, IWorldPool argPool)
{
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float tmin = Single.Epsilon;
float tmax = Single.MaxValue;
Vec2 p = argPool.popVec2();
Vec2 d = argPool.popVec2();
Vec2 absD = argPool.popVec2();
Vec2 normal = argPool.popVec2();
p.set_Renamed(input.p1);
d.set_Renamed(input.p2).subLocal(input.p1);
Vec2.absToOut(d, absD);
// x then y
if (absD.x < Settings.EPSILON)
{
// Parallel.
if (p.x < lowerBound.x || upperBound.x < p.x)
{
argPool.pushVec2(4);
return false;
}
}
else
{
float inv_d = 1.0f / d.x;
float t1 = (lowerBound.x - p.x) * inv_d;
float t2 = (upperBound.x - p.x) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.setZero();
normal.x = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.min(tmax, t2);
if (tmin > tmax)
{
argPool.pushVec2(4);
return false;
}
}
if (absD.y < Settings.EPSILON)
{
// Parallel.
if (p.y < lowerBound.y || upperBound.y < p.y)
{
argPool.pushVec2(4);
return false;
}
}
else
{
float inv_d = 1.0f / d.y;
float t1 = (lowerBound.y - p.y) * inv_d;
float t2 = (upperBound.y - p.y) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
float temp = t1;
t1 = t2;
t2 = temp;
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.setZero();
normal.y = s;
tmin = t1;
}
// Pull the max down
tmax = MathUtils.min(tmax, t2);
if (tmin > tmax)
{
argPool.pushVec2(4);
return false;
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
argPool.pushVec2(4);
return false;
}
// Intersection.
output.fraction = tmin;
output.normal.x = normal.x;
output.normal.y = normal.y;
argPool.pushVec2(4);
return true;
}
public EdgeAndCircleContact(IWorldPool argPool)
: base(argPool)
{
}
public ChainAndPolygonContact(IWorldPool argPool)
: base(argPool)
{
}
public DefaultWorldPool(int argSize, int argContainerSize)
{
world = this;
pcstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new PolygonContact(world)
};
ccstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new CircleContact(world)
};
cpstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new PolygonAndCircleContact(world)
};
ecstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new EdgeAndCircleContact(world)
};
epstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new EdgeAndPolygonContact(world)
};
chcstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new ChainAndCircleContact(world)
};
chpstack = new MutableStack <Contact>(Settings.CONTACT_STACK_INIT_SIZE)
{
newInstance = () => new ChainAndPolygonContact(world)
};
vecs = new OrderedStack <Vec2>(argSize, argContainerSize)
{
newInstance = () => new Vec2()
};
vec3s = new OrderedStack <Vec3>(argSize, argContainerSize)
{
newInstance = () => new Vec3()
};
mats = new OrderedStack <Mat22>(argSize, argContainerSize)
{
newInstance = () => new Mat22()
};
aabbs = new OrderedStack <AABB>(argSize, argContainerSize)
{
newInstance = () => new AABB()
};
rots = new OrderedStack <Rot>(argSize, argContainerSize)
{
newInstance = () => new Rot()
};
mat33s = new OrderedStack <Mat33>(argSize, argContainerSize)
{
newInstance = () => new Mat33()
};
dist = new Distance();
collision = new Collision(this);
toi = new TimeOfImpact(this);
}
public ChainAndPolygonContact(IWorldPool argPool) : base(argPool)
{
}
public PolyShapesCallback(IWorldPool argWorld)
{
m_count = 0;
p = argWorld;
}
public RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
LocalAnchorA.Set(def.LocalAnchorA);
LocalAnchorB.Set(def.LocalAnchorB);
ReferenceAngle = def.ReferenceAngle;
MotorImpulse = 0;
LowerAngle = def.LowerAngle;
UpperAngle = def.UpperAngle;
m_maxMotorTorque = def.MaxMotorTorque;
m_motorSpeed = def.MotorSpeed;
m_limitEnabled = def.EnableLimit;
m_motorEnabled = def.EnableMotor;
LimitState = LimitState.Inactive;
}
public PolygonAndCircleContact(IWorldPool argPool)
: base(argPool)
{
}
public TimeOfImpact(IWorldPool argPool)
{
pool = argPool;
}
public CircleContact(IWorldPool argPool)
: base(argPool)
{
}
/// <param name="argWorld"></param>
/// <param name="def"></param>
protected internal WeldJoint(IWorldPool argWorld, WeldJointDef def)
: base(argWorld, def)
{
LocalAnchorA = new Vec2(def.LocalAnchorA);
LocalAnchorB = new Vec2(def.LocalAnchorB);
m_referenceAngle = def.ReferenceAngle;
Frequency = def.FrequencyHz;
DampingRatio = def.DampingRatio;
m_impulse = new Vec3();
m_impulse.SetZero();
}
internal WheelJoint(IWorldPool argPool, WheelJointDef def)
: base(argPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_localXAxisA.set(def.localAxisA);
Vec2.crossToOutUnsafe(1.0f, m_localXAxisA, ref m_localYAxisA);
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableMotor = def.enableMotor;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
}
public EdgeAndCircleContact(IWorldPool argPool) : base(argPool)
{
}
/// <param name="argWorld"></param>
/// <param name="def"></param>
protected internal WeldJoint(IWorldPool argWorld, WeldJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_frequencyHz = def.frequencyHz;
m_dampingRatio = def.dampingRatio;
m_impulse = new Vec3();
m_impulse.setZero();
}
public PolygonAndCircleContact(IWorldPool argPool)
: base(argPool)
{
}
public EdgeAndPolygonContact(IWorldPool argPool)
: base(argPool)
{
}
public TimeOfImpact(IWorldPool argPool)
{
pool = argPool;
}
