//--------------- Internals Below -------------------
static public b2Joint Create(b2JointDef def, object allocator)
{
b2Joint joint = null;
if (def.type == e_distanceJoint)
{
//void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
joint = new b2DistanceJoint(def as b2DistanceJointDef);
}
else if (def.type == e_mouseJoint)
{
//void* mem = allocator->Allocate(sizeof(b2MouseJoint));
joint = new b2MouseJoint(def as b2MouseJointDef);
}
else if (def.type == e_prismaticJoint)
{
//void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
joint = new b2PrismaticJoint(def as b2PrismaticJointDef);
}
else if (def.type == e_revoluteJoint)
{
//void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
joint = new b2RevoluteJoint(def as b2RevoluteJointDef);
}
else if (def.type == e_pulleyJoint)
{
//void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
joint = new b2PulleyJoint(def as b2PulleyJointDef);
}
else if (def.type == e_gearJoint)
{
//void* mem = allocator->Allocate(sizeof(b2GearJoint));
joint = new b2GearJoint(def as b2GearJointDef);
}
else if (def.type == e_lineJoint)
{
//void* mem = allocator->Allocate(sizeof(b2LineJoint));
joint = new b2LineJoint(def as b2LineJointDef);
}
else if (def.type == e_weldJoint)
{
//void* mem = allocator->Allocate(sizeof(b2WeldJoint));
joint = new b2WeldJoint(def as b2WeldJointDef);
}
else if (def.type == e_frictionJoint)
{
//void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
joint = new b2FrictionJoint(def as b2FrictionJointDef);
}
else if (def.type == e_ropeJoint)
{
joint = new b2RopeJoint(def as b2RopeJointDef);
}
else
{
//b2Settings.b2Assert(false);
}
return(joint);
}
public override void SayGoodbye(b2Joint joint)
{
if (test.m_mouseJoint == joint)
{
test.m_mouseJoint = null;
}
else
{
test.JointDestroyed(joint);
}
}
/// Called when any joint is about to be destroyed due
/// to the destruction of one of its attached bodies.
public override void SayGoodbye(b2Joint joint)
{
if (m_mouseJoint == joint)
{
m_mouseJoint = null;
}
else
{
// JointDestroyed(joint);
}
Console.WriteLine("ByeBye Joint: {0}", joint);
}
public b2Joint(b2JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
m_type = def.JointType;
Prev = null;
Next = null;
m_bodyA = def.BodyA;
m_bodyB = def.BodyB;
m_index = 0;
m_collideConnected = def.CollideConnected;
m_islandFlag = false;
m_userData = def.UserData;
m_edgeA = new b2JointEdge();
m_edgeB = new b2JointEdge();
}
static public void Destroy(b2Joint joint, object allocator)
{
/*joint->~b2Joint();
* switch (joint.m_type)
* {
* case e_distanceJoint:
* allocator->Free(joint, sizeof(b2DistanceJoint));
* break;
*
* case e_mouseJoint:
* allocator->Free(joint, sizeof(b2MouseJoint));
* break;
*
* case e_prismaticJoint:
* allocator->Free(joint, sizeof(b2PrismaticJoint));
* break;
*
* case e_revoluteJoint:
* allocator->Free(joint, sizeof(b2RevoluteJoint));
* break;
*
* case e_pulleyJoint:
* allocator->Free(joint, sizeof(b2PulleyJoint));
* break;
*
* case e_gearJoint:
* allocator->Free(joint, sizeof(b2GearJoint));
* break;
*
* case e_lineJoint:
* allocator->Free(joint, sizeof(b2LineJoint));
* break;
*
* case e_weldJoint:
* allocator->Free(joint, sizeof(b2WeldJoint));
* break;
*
* case e_frictionJoint:
* allocator->Free(joint, sizeof(b2FrictionJoint));
* break;
*
* default:
* b2Assert(false);
* break;
* }*/
}
public b2Joint(b2JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
m_type = def.JointType;
Prev = null;
Next = null;
m_bodyA = def.BodyA;
m_bodyB = def.BodyB;
m_index = 0;
m_collideConnected = def.CollideConnected;
m_islandFlag = false;
m_userData = def.UserData;
m_edgeA.Joint = null;
m_edgeA.Other = null;
m_edgeA.Prev = null;
m_edgeA.Next = null;
m_edgeB.Joint = null;
m_edgeB.Other = null;
m_edgeB.Prev = null;
m_edgeB.Next = null;
}
public JObject B2n(b2Joint joint)
{
JObject jointValue = new JObject();
String jointName = GetJointName(joint);
if (null != jointName)
jointValue["name"] = jointName;
int bodyIndexA = lookupBodyIndex(joint.GetBodyA());
int bodyIndexB = lookupBodyIndex(joint.GetBodyB());
jointValue["bodyA"] = bodyIndexA;
jointValue["bodyB"] = bodyIndexB;
if (joint.GetCollideConnected())
jointValue["collideConnected"] = true;
b2Body bodyA = joint.GetBodyA();
b2Body bodyB = joint.GetBodyB();
// why do Joint.getAnchor methods need to take an argOut style
// parameter!?
b2Vec2 tmpAnchor = new b2Vec2();
switch (joint.GetJointType())
{
case b2JointType.e_revoluteJoint:
{
jointValue["type"] = "revolute";
b2RevoluteJoint revoluteJoint = (b2RevoluteJoint)joint;
tmpAnchor = revoluteJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = revoluteJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("refAngle", bodyB.Angle - bodyA.Angle - revoluteJoint.GetJointAngle(), jointValue);
FloatToJson("jointSpeed", revoluteJoint.GetJointSpeed(), jointValue);
jointValue["enableLimit"] = revoluteJoint.IsLimitEnabled();
FloatToJson("lowerLimit", revoluteJoint.GetLowerLimit(), jointValue);
FloatToJson("upperLimit", revoluteJoint.GetUpperLimit(), jointValue);
jointValue["enableMotor"] = revoluteJoint.IsMotorEnabled();
FloatToJson("motorSpeed", revoluteJoint.GetMotorSpeed(), jointValue);
FloatToJson("maxMotorTorque", revoluteJoint.GetMaxMotorTorque(), jointValue);
}
break;
case b2JointType.e_prismaticJoint:
{
jointValue["type"] = "prismatic";
b2PrismaticJoint prismaticJoint = (b2PrismaticJoint)joint;
tmpAnchor = prismaticJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = prismaticJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
VecToJson("localAxisA", prismaticJoint.GetLocalXAxisA(), jointValue);
FloatToJson("refAngle", prismaticJoint.GetReferenceAngle(), jointValue);
jointValue["enableLimit"] = prismaticJoint.IsLimitEnabled();
FloatToJson("lowerLimit", prismaticJoint.GetLowerLimit(), jointValue);
FloatToJson("upperLimit", prismaticJoint.GetUpperLimit(), jointValue);
jointValue["enableMotor"] = prismaticJoint.IsMotorEnabled();
FloatToJson("maxMotorForce", prismaticJoint.GetMaxMotorForce(), jointValue);
FloatToJson("motorSpeed", prismaticJoint.GetMotorSpeed(), jointValue);
}
break;
case b2JointType.e_distanceJoint:
{
jointValue["type"] = "distance";
b2DistanceJoint distanceJoint = (b2DistanceJoint)joint;
tmpAnchor = distanceJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = distanceJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
//TODO: UNKOWS METHODS
//FloatToJson("length", distanceJoint.GetLenght(), jointValue);
//FloatToJson("frequency", distanceJoint.GetFrequency(), jointValue);
//FloatToJson("dampingRatio", distanceJoint.GetDampingRatio(), jointValue);
}
break;
case b2JointType.e_pulleyJoint:
{
jointValue["type"] = "pulley";
b2PulleyJoint pulleyJoint = (b2PulleyJoint)joint;
VecToJson("groundAnchorA", pulleyJoint.GetGroundAnchorA(), jointValue);
VecToJson("groundAnchorB", pulleyJoint.GetGroundAnchorB(), jointValue);
tmpAnchor = pulleyJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("lengthA", (pulleyJoint.GetGroundAnchorA() - tmpAnchor).Length, jointValue);
tmpAnchor = pulleyJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("lengthB", (pulleyJoint.GetGroundAnchorB() - tmpAnchor).Length, jointValue);
FloatToJson("ratio", pulleyJoint.GetRatio(), jointValue);
}
break;
case b2JointType.e_mouseJoint:
{
jointValue["type"] = "mouse";
b2MouseJoint mouseJoint = (b2MouseJoint)joint;
VecToJson("target", mouseJoint.GetTarget(), jointValue);
tmpAnchor = mouseJoint.GetAnchorB();
VecToJson("anchorB", tmpAnchor, jointValue);
FloatToJson("maxForce", mouseJoint.GetMaxForce(), jointValue);
FloatToJson("frequency", mouseJoint.GetFrequency(), jointValue);
FloatToJson("dampingRatio", mouseJoint.GetDampingRatio(), jointValue);
}
break;
case b2JointType.e_gearJoint:
{
jointValue["type"] = "gear";
b2GearJoint gearJoint = (b2GearJoint)joint;
int jointIndex1 = lookupJointIndex(gearJoint.GetJoint1());
int jointIndex2 = lookupJointIndex(gearJoint.GetJoint2());
jointValue["joint1"] = jointIndex1;
jointValue["joint2"] = jointIndex2;
jointValue["ratio"] = gearJoint.GetRatio();
}
break;
case b2JointType.e_wheelJoint:
{
jointValue["type"] = "wheel";
b2WheelJoint wheelJoint = (b2WheelJoint)joint;
tmpAnchor = wheelJoint.GetAnchorA();
VecToJson("anchorA", tmpAnchor, jointValue);
tmpAnchor = wheelJoint.GetAnchorB();
VecToJson("anchorB", tmpAnchor, jointValue);
//TODO: UNKNOWN METHOD
//VecToJson("localAxisA", wheelJoint.GetLocalAxisA(), jointValue);
jointValue["enableMotor"] = wheelJoint.IsMotorEnabled();
FloatToJson("motorSpeed", wheelJoint.GetMotorSpeed(), jointValue);
FloatToJson("maxMotorTorque", wheelJoint.GetMaxMotorTorque(), jointValue);
FloatToJson("springFrequency", wheelJoint.GetSpringFrequencyHz(), jointValue);
FloatToJson("springDampingRatio", wheelJoint.GetSpringDampingRatio(), jointValue);
}
break;
case b2JointType.e_weldJoint:
{
jointValue["type"] = "weld";
b2WeldJoint weldJoint = (b2WeldJoint)joint;
tmpAnchor = weldJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = weldJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("refAngle", weldJoint.ReferenceAngle, jointValue);
FloatToJson("frequency", weldJoint.Frequency, jointValue);
FloatToJson("dampingRatio", weldJoint.DampingRatio, jointValue);
}
break;
case b2JointType.e_frictionJoint:
{
jointValue["type"] = "friction";
b2FrictionJoint frictionJoint = (b2FrictionJoint)joint;
tmpAnchor = frictionJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = frictionJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("maxForce", frictionJoint.GetMaxForce(), jointValue);
FloatToJson("maxTorque", frictionJoint.GetMaxTorque(), jointValue);
}
break;
case b2JointType.e_ropeJoint:
{
// Rope joints are apparently not implemented in JBox2D yet, but
// when they are, commenting out the following section should work.
jointValue["type"] = "rope";
b2RopeJoint ropeJoint = (b2RopeJoint)joint;
tmpAnchor = ropeJoint.GetAnchorA();
VecToJson("anchorA", bodyA.GetLocalPoint(tmpAnchor), jointValue);
tmpAnchor = ropeJoint.GetAnchorB();
VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
FloatToJson("maxLength", ropeJoint.GetMaxLength(), jointValue);
}
break;
//case b2JointType.Motor:
//{
// jointValue["type"] = "motor";
// MotorJoint motor = (MotorJoint)joint;
// tmpAnchor = motor.WorldAnchorA;
// VecToJson("anchorA", motor.LinearOffset, jointValue);
// //tmpAnchor = motor.WorldAnchorB;
// //VecToJson("anchorB", bodyB.GetLocalPoint(tmpAnchor), jointValue);
// FloatToJson("maxForce", motor.MaxForce, jointValue);
// FloatToJson("maxTorque", motor.MaxTorque, jointValue);
// FloatToJson("refAngle", motor.AngularOffset, jointValue);
//}
//break;
default:
Console.WriteLine("Unknown joint type : " + joint.GetJointType());
break;
}
JArray customPropertyValue = WriteCustomPropertiesToJson(joint);
if (customPropertyValue.Count > 0)
jointValue["customProperties"] = customPropertyValue;
return jointValue;
}
public override void JointDestroyed(b2Joint joint)
{
for (int i = 0; i < 8; ++i)
{
if (m_joints[i] == joint)
{
m_joints[i] = null;
break;
}
}
}
public virtual void JointDestroyed(b2Joint joint)
{
}
public b2GearJoint(b2GearJointDef def)
: base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetJointType();
m_typeB = m_joint2.GetJointType();
Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1.GetBodyA();
m_bodyA = m_joint1.GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA.XF;
float aA = m_bodyA.Sweep.a;
b2Transform xfC = m_bodyC.XF;
float aC = m_bodyC.Sweep.a;
if (m_typeA == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint1;
m_localAnchorC = revolute.GetLocalAnchorA();
m_localAnchorA = revolute.GetLocalAnchorB();
m_referenceAngleA = revolute.GetReferenceAngle();
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.GetLocalAnchorA();
m_localAnchorA = prismatic.GetLocalAnchorB();
m_referenceAngleA = prismatic.GetReferenceAngle();
m_localAxisC = prismatic.GetLocalXAxisA();
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2Math.b2MulT(xfC.q, b2Math.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Math.b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.GetBodyA();
m_bodyB = m_joint2.GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB.XF;
float aB = m_bodyB.Sweep.a;
b2Transform xfD = m_bodyD.XF;
float aD = m_bodyD.Sweep.a;
if (m_typeB == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint2;
m_localAnchorD = revolute.GetLocalAnchorA();
m_localAnchorB = revolute.GetLocalAnchorB();
m_referenceAngleB = revolute.GetReferenceAngle();
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.GetLocalAnchorA();
m_localAnchorB = prismatic.GetLocalAnchorB();
m_referenceAngleB = prismatic.GetReferenceAngle();
m_localAxisD = prismatic.GetLocalXAxisA();
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2Math.b2MulT(xfD.q, b2Math.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Math.b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
public Wheel()
{
this.Initialize = car =>
{
/*
wheel object
pars:
car - car this wheel belongs to
x - horizontal position in meters relative to car's center
y - vertical position in meters relative to car's center
width - width in meters
length - length in meters
revolving - does this wheel revolve when steering?
powered - is this wheel powered?
*/
var position = new double[] { x, y };
//this.car=pars.car;
//this.revolving=pars.revolving;
//this.powered=pars.powered;
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = car.body.GetWorldPoint(new b2Vec2(position[0], position[1]));
def.angle = car.body.GetAngle();
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1;
fixdef.isSensor = true; //wheel does not participate in collision calculations: resulting complications are unnecessary
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
this.fix = body.CreateFixture(fixdef);
var jointdef = new b2RevoluteJointDef();
//create joint to connect wheel to body
if (revolving)
{
jointdef.Initialize(car.body, body, body.GetWorldCenter());
jointdef.enableMotor = false; //we'll be controlling the wheel's angle manually
}
else
{
jointdef.Initialize(car.body, body, body.GetWorldCenter()
//, new b2Vec2(1, 0)
);
jointdef.enableLimit = true;
//jointdef.lowerTranslation = 0;
//jointdef.upperTranslation = 0;
}
joint = b2world.CreateJoint(jointdef);
#region setAngle
this.setAngle +=
(angle) =>
{
this.rotation = angle.DegreesToRadians();
/*
angle - wheel angle relative to car, in degrees
*/
body.SetAngle(car.body.GetAngle() + angle.DegreesToRadians());
};
#endregion
#region getLocalVelocity
Func<double[]> getLocalVelocity = delegate
{
/*returns get velocity vector relative to car
*/
var res = car.body.GetLocalVector(car.body.GetLinearVelocityFromLocalPoint(new b2Vec2(position[0], position[1])));
return new double[] { res.x, res.y };
};
#endregion
#region getDirectionVector
Func<double[]> getDirectionVector = delegate
{
/*
returns a world unit vector pointing in the direction this wheel is moving
*/
if (getLocalVelocity()[1] > 0)
return vectors.rotate(new double[] { 0, 1 }, body.GetAngle());
else
return vectors.rotate(new double[] { 0, -1 }, body.GetAngle());
};
#endregion
#region getKillVelocityVector
Func<double[]> getKillVelocityVector = delegate
{
/*
substracts sideways velocity from this wheel's velocity vector and returns the remaining front-facing velocity vector
*/
var velocity = body.GetLinearVelocity();
var sideways_axis = getDirectionVector();
var dotprod = vectors.dot(new[] { velocity.x, velocity.y }, sideways_axis);
return new double[] { sideways_axis[0] * dotprod, sideways_axis[1] * dotprod };
};
#endregion
#region killSidewaysVelocity
this.killSidewaysVelocity = delegate
{
/*
removes all sideways velocity from this wheels velocity
*/
var kv = getKillVelocityVector();
body.SetLinearVelocity(new b2Vec2(kv[0], kv[1]));
};
#endregion
};
}
public void SetCustomVector(b2Joint item, String propertyName, b2Vec2 val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_vec2.Add(propertyName, val);
}
public b2World(b2Vec2 gravity)
{
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 = gravity;
m_flags = b2WorldFlags.e_clearForces;
m_inv_dt0 = 0.0f;
// setup up our default Contact Manager
m_contactManager = new b2ContactManager();
}
public void SetCustomFloat(b2Joint item, String propertyName, float val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_float.Add(propertyName, (float)val);
}
public void SetCustomString(b2Joint item, String propertyName, String val)
{
m_jointsWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_string.Add(propertyName, val);
}
public b2GearJoint(b2GearJointDef def)
: base(def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.GetJointType();
m_typeB = m_joint2.GetJointType();
Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1.GetBodyA();
m_bodyA = m_joint1.GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA.XF;
float aA = m_bodyA.Sweep.a;
b2Transform xfC = m_bodyC.XF;
float aC = m_bodyC.Sweep.a;
if (m_typeA == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint1;
m_localAnchorC = revolute.GetLocalAnchorA();
m_localAnchorA = revolute.GetLocalAnchorB();
m_referenceAngleA = revolute.GetReferenceAngle();
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;
m_localAnchorC = prismatic.GetLocalAnchorA();
m_localAnchorA = prismatic.GetLocalAnchorB();
m_referenceAngleA = prismatic.GetReferenceAngle();
m_localAxisC = prismatic.GetLocalXAxisA();
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2Math.b2MulT(xfC.q, b2Math.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Math.b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2.GetBodyA();
m_bodyB = m_joint2.GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB.XF;
float aB = m_bodyB.Sweep.a;
b2Transform xfD = m_bodyD.XF;
float aD = m_bodyD.Sweep.a;
if (m_typeB == b2JointType.e_revoluteJoint)
{
b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint2;
m_localAnchorD = revolute.GetLocalAnchorA();
m_localAnchorB = revolute.GetLocalAnchorB();
m_referenceAngleB = revolute.GetReferenceAngle();
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;
m_localAnchorD = prismatic.GetLocalAnchorA();
m_localAnchorB = prismatic.GetLocalAnchorB();
m_referenceAngleB = prismatic.GetReferenceAngle();
m_localAxisD = prismatic.GetLocalXAxisA();
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2Math.b2MulT(xfD.q, b2Math.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Math.b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
public DumpShell()
{
b2Vec2 g = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
m_world.Gravity = g;
b2Body[] bodies = new b2Body[3];
b2Joint[] joints = new b2Joint[2];
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(1.304347801208496e+01f, 2.500000000000000e+00f);
bd.angle = 0.000000000000000e+00f;
bd.linearVelocity.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
bd.angularVelocity = 0.000000000000000e+00f;
bd.linearDamping = 5.000000000000000e-01f;
bd.angularDamping = 5.000000000000000e-01f;
bd.allowSleep = true;
bd.awake = true;
bd.fixedRotation = false;
bd.bullet = false;
bd.active = true;
bd.gravityScale = 1.000000000000000e+00f;
bodies[0] = m_world.CreateBody(bd);
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+00f;
fd.restitution = 5.000000000000000e-01f;
fd.density = 1.000000000000000e+01f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2PolygonShape shape = new b2PolygonShape();
b2Vec2[] vs = new b2Vec2[8];
vs[0].Set(-6.900000095367432e+00f, -3.000000119209290e-01f);
vs[1].Set(2.000000029802322e-01f, -3.000000119209290e-01f);
vs[2].Set(2.000000029802322e-01f, 2.000000029802322e-01f);
vs[3].Set(-6.900000095367432e+00f, 2.000000029802322e-01f);
shape.Set(vs, 4);
fd.shape = shape;
bodies[0].CreateFixture(fd);
}
}
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(8.478260636329651e-01f, 2.500000000000000e+00f);
bd.angle = 0.000000000000000e+00f;
bd.linearVelocity.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
bd.angularVelocity = 0.000000000000000e+00f;
bd.linearDamping = 5.000000000000000e-01f;
bd.angularDamping = 5.000000000000000e-01f;
bd.allowSleep = true;
bd.awake = true;
bd.fixedRotation = false;
bd.bullet = false;
bd.active = true;
bd.gravityScale = 1.000000000000000e+00f;
bodies[1] = m_world.CreateBody(bd);
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+00f;
fd.restitution = 5.000000000000000e-01f;
fd.density = 1.000000000000000e+01f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2PolygonShape shape = new b2PolygonShape();
b2Vec2[] vs = new b2Vec2[8];
vs[0].Set(-3.228000104427338e-01f, -2.957000136375427e-01f);
vs[1].Set(6.885900020599365e+00f, -3.641000092029572e-01f);
vs[2].Set(6.907599925994873e+00f, 3.271999955177307e-01f);
vs[3].Set(-3.228000104427338e-01f, 2.825999855995178e-01f);
shape.Set(vs, 4);
fd.shape = shape;
bodies[1].CreateFixture(fd);
}
}
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_staticBody;
bd.position.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
bd.angle = 0.000000000000000e+00f;
bd.linearVelocity.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
bd.angularVelocity = 0.000000000000000e+00f;
bd.linearDamping = 0.000000000000000e+00f;
bd.angularDamping = 0.000000000000000e+00f;
bd.allowSleep = true;
bd.awake = true;
bd.fixedRotation = false;
bd.bullet = false;
bd.active = true;
bd.gravityScale = 1.000000000000000e+00f;
bodies[2] = m_world.CreateBody(bd);
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+01f;
fd.restitution = 0.000000000000000e+00f;
fd.density = 0.000000000000000e+00f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2EdgeShape shape = new b2EdgeShape();
shape.Radius = 9.999999776482582e-03f;
shape.Vertex0 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex1 = new b2Vec2(4.452173995971680e+01f, 1.669565200805664e+01f);
shape.Vertex2 = new b2Vec2(4.452173995971680e+01f, 0.000000000000000e+00f);
shape.Vertex3 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.HasVertex0 = false;
shape.HasVertex3 = false;
fd.shape = shape;
bodies[2].CreateFixture(fd);
}
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+01f;
fd.restitution = 0.000000000000000e+00f;
fd.density = 0.000000000000000e+00f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2EdgeShape shape = new b2EdgeShape();
shape.Radius = 9.999999776482582e-03f;
shape.Vertex0 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex1 = new b2Vec2(0.000000000000000e+00f, 1.669565200805664e+01f);
shape.Vertex2 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex3 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.HasVertex0 = false;
shape.HasVertex3 = false;
fd.shape = shape;
bodies[2].CreateFixture(fd);
}
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+01f;
fd.restitution = 0.000000000000000e+00f;
fd.density = 0.000000000000000e+00f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2EdgeShape shape = new b2EdgeShape();
shape.Radius = 9.999999776482582e-03f;
shape.Vertex0 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex1 = new b2Vec2(0.000000000000000e+00f, 1.669565200805664e+01f);
shape.Vertex2 = new b2Vec2(4.452173995971680e+01f, 1.669565200805664e+01f);
shape.Vertex3 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.HasVertex0 = false;
shape.HasVertex3 = false;
fd.shape = shape;
bodies[2].CreateFixture(fd);
}
{
b2FixtureDef fd = new b2FixtureDef();
fd.friction = 1.000000000000000e+01f;
fd.restitution = 0.000000000000000e+00f;
fd.density = 0.000000000000000e+00f;
fd.isSensor = false;
fd.filter.categoryBits = 1;
fd.filter.maskBits = 65535;
fd.filter.groupIndex = 0;
b2EdgeShape shape = new b2EdgeShape();
shape.Radius = 9.999999776482582e-03f;
shape.Vertex0 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex1 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.Vertex2 = new b2Vec2(4.452173995971680e+01f, 0.000000000000000e+00f);
shape.Vertex3 = new b2Vec2(0.000000000000000e+00f, 0.000000000000000e+00f);
shape.HasVertex0 = false;
shape.HasVertex3 = false;
fd.shape = shape;
bodies[2].CreateFixture(fd);
}
}
{
b2PrismaticJointDef jd = new b2PrismaticJointDef();
jd.BodyA = bodies[1];
jd.BodyB = bodies[0];
jd.CollideConnected = false;
jd.localAnchorA.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
jd.localAnchorB.Set(-1.219565200805664e+01f, 0.000000000000000e+00f);
jd.localAxisA.Set(-1.219565200805664e+01f, 0.000000000000000e+00f);
jd.referenceAngle = 0.000000000000000e+00f;
jd.enableLimit = true;
jd.lowerTranslation = -2.000000000000000e+01f;
jd.upperTranslation = 0.000000000000000e+00f;
jd.enableMotor = true;
jd.motorSpeed = 0.000000000000000e+00f;
jd.maxMotorForce = 1.000000000000000e+01f;
joints[0] = m_world.CreateJoint(jd);
}
{
b2RevoluteJointDef jd = new b2RevoluteJointDef();
jd.BodyA = bodies[1];
jd.BodyB = bodies[2];
jd.CollideConnected = false;
jd.localAnchorA.Set(0.000000000000000e+00f, 0.000000000000000e+00f);
jd.localAnchorB.Set(8.478260636329651e-01f, 2.500000000000000e+00f);
jd.referenceAngle = 0.000000000000000e+00f;
jd.enableLimit = false;
jd.lowerAngle = 0.000000000000000e+00f;
jd.upperAngle = 0.000000000000000e+00f;
jd.enableMotor = false;
jd.motorSpeed = 0.000000000000000e+00f;
jd.maxMotorTorque = 0.000000000000000e+00f;
joints[1] = m_world.CreateJoint(jd);
}
}
protected void readCustomPropertiesFromJson(b2Joint item, JObject value)
{
if (null == item)
return;
if (value["customProperties"] != null)
return;
int i = 0;
JArray propValues = (JArray)value["customProperties"];
if (null != propValues)
{
int numPropValues = propValues.Count;
for (i = 0; i < numPropValues; i++)
{
JObject propValue = (JObject)propValues[i];
String propertyName = propValue["name"].ToString();
if (propValue["int"] != null)
SetCustomInt(item, propertyName, (int)propValue["int"]);
if (propValue["float"] != null)
SetCustomFloat(item, propertyName, (float)propValue["float"]);
if (propValue["string"] != null)
SetCustomString(item, propertyName, propValue["string"].ToString());
if (propValue["vec2"] != null)
SetCustomVector(item, propertyName, this.jsonToVec("vec2", propValue));
if (propValue["bool"] != null)
SetCustomBool(item, propertyName, (bool)propValue["bool"]);
}
}
}
protected int lookupJointIndex(b2Joint joint)
{
int? val = m_jointToIndexMap[joint];
if (null != val)
return val.Value;
else
return -1;
}
public void SetJointName(b2Joint joint, String name)
{
m_jointToNameMap.Add(joint, name);
}
/// Called when any joint is about to be destroyed due /// to the destruction of one of its attached bodies. public abstract void SayGoodbye(b2Joint joint);
public static b2Joint Create(b2JointDef def)
{
b2Joint joint = null;
switch (def.JointType)
{
case b2JointType.e_distanceJoint:
{
joint = new b2DistanceJoint((b2DistanceJointDef)def);
}
break;
case b2JointType.e_mouseJoint:
{
joint = new b2MouseJoint((b2MouseJointDef)def);
}
break;
case b2JointType.e_prismaticJoint:
{
joint = new b2PrismaticJoint((b2PrismaticJointDef)def);
}
break;
case b2JointType.e_revoluteJoint:
{
joint = new b2RevoluteJoint((b2RevoluteJointDef)def);
}
break;
case b2JointType.e_pulleyJoint:
{
joint = new b2PulleyJoint((b2PulleyJointDef)def);
}
break;
case b2JointType.e_gearJoint:
{
joint = new b2GearJoint((b2GearJointDef)def);
}
break;
case b2JointType.e_wheelJoint:
{
joint = new b2WheelJoint((b2WheelJointDef)def);
}
break;
case b2JointType.e_weldJoint:
{
joint = new b2WeldJoint((b2WeldJointDef)def);
}
break;
case b2JointType.e_frictionJoint:
{
joint = new b2FrictionJoint((b2FrictionJointDef)def);
}
break;
case b2JointType.e_ropeJoint:
{
joint = new b2RopeJoint((b2RopeJointDef)def);
}
break;
default:
Debug.Assert(false);
break;
}
return(joint);
}
public void Add(b2Joint joint)
{
Debug.Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount++] = joint;
}
public String GetJointName(b2Joint joint)
{
if (m_jointToNameMap.ContainsKey(joint))
return m_jointToNameMap[joint];
return null;
}