public b2Joint CreateJoint(b2JointDef def)
{
global::System.IntPtr cPtr = Box2dPINVOKE.b2World_CreateJoint(swigCPtr, b2JointDef.getCPtr(def));
b2Joint ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Joint(cPtr, false);
return(ret);
}
public b2Joint GetJoint2()
{
global::System.IntPtr cPtr = Box2dPINVOKE.b2GearJoint_GetJoint2(swigCPtr);
b2Joint ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Joint(cPtr, false);
return(ret);
}
public b2Joint GetNext()
{
global::System.IntPtr cPtr = Box2DPINVOKE.b2Joint_GetNext__SWIG_0(swigCPtr);
b2Joint ret = (cPtr == global::System.IntPtr.Zero) ? null : new b2Joint(cPtr, false);
return(ret);
}
public void Dump()
{
if ((m_flags & b2WorldType.e_locked) == e_locked)
{
return;
}
System.Diagnostics.Debug.WriteLine("b2Vec2 g({0:N5}, {0:N5});", m_gravity.x, m_gravity.y);
System.Diagnostics.Debug.WriteLine("m_world.SetGravity(g);");
System.Diagnostics.Debug.WriteLine("b2Body* bodies = (b2Body*)b2Alloc({0} * sizeof(b2Body));", m_bodyCount);
System.Diagnostics.Debug.WriteLine("b2Joint* joints = (b2Joint*)b2Alloc({0} * sizeof(b2Joint));", m_jointCount);
int i = 0;
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b.IslandIndex = i;
b.Dump();
++i;
}
i = 0;
for (b2Joint j = m_jointList; j != null; j = j.Next)
{
j.Index = i;
++i;
}
// First pass on joints, skip gear joints.
for (b2Joint j = m_jointList; j != null; j = j.Next)
{
if (j.JointType == b2JointType.e_gearJoint)
{
continue;
}
System.Diagnostics.Debug.WriteLine("{");
j.Dump();
System.Diagnostics.Debug.WriteLine("}");
}
// Second pass on joints, only gear joints.
for (b2Joint j = m_jointList; j != null; j = j.Next)
{
if (j.JointType != b2JointType.e_gearJoint)
{
continue;
}
System.Diagnostics.Debug.WriteLine("{");
j.Dump();
System.Diagnostics.Debug.WriteLine("}");
}
System.Diagnostics.Debug.WriteLine("b2Free(joints);");
System.Diagnostics.Debug.WriteLine("b2Free(bodies);");
System.Diagnostics.Debug.WriteLine("joints = null;");
System.Diagnostics.Debug.WriteLine("bodies = null;");
}
public override void SayGoodbye(b2Joint joint)
{
if (test.m_mouseJoint == joint)
{
test.m_mouseJoint = null;
}
else
{
test.JointDestroyed(joint);
}
}
public override void JointDestroyed(b2Joint joint)
{
for (int i = 0; i < 8; ++i)
{
if (m_joints[i] == joint)
{
m_joints[i] = null;
break;
}
}
}
/*
* Position Correction Notes
* =========================
* I tried the several algorithms for position correction of the 2D revolute joint.
* I looked at these systems:
* - simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
* - suspension bridge with 30 1m long planks of length 1m.
* - multi-link chain with 30 1m long links.
*
* Here are the algorithms:
*
* Baumgarte - A fraction of the position error is added to the velocity error. There is no
* separate position solver.
*
* Pseudo Velocities - After the velocity solver and position integration,
* the position error, Jacobian, and effective mass are recomputed. Then
* the velocity constraints are solved with pseudo velocities and a fraction
* of the position error is added to the pseudo velocity error. The pseudo
* velocities are initialized to zero and there is no warm-starting. After
* the position solver, the pseudo velocities are added to the positions.
* This is also called the First Order World method or the Position LCP method.
*
* Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
* position error is re-computed for each constraint and the positions are updated
* after the constraint is solved. The radius vectors (aka Jacobians) are
* re-computed too (otherwise the algorithm has horrible instability). The pseudo
* velocity states are not needed because they are effectively zero at the beginning
* of each iteration. Since we have the current position error, we allow the
* iterations to terminate early if the error becomes smaller than b2_linearSlop.
*
* Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
* each time a constraint is solved.
*
* Here are the results:
* Baumgarte - this is the cheapest algorithm but it has some stability problems,
* especially with the bridge. The chain links separate easily close to the root
* and they jitter as they struggle to pull together. This is one of the most common
* methods in the field. The big drawback is that the position correction artificially
* affects the momentum, thus leading to instabilities and false bounce. I used a
* bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
* factor makes joints and contacts more spongy.
*
* Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
* stable. However, joints still separate with large angular velocities. Drag the
* simple pendulum in a circle quickly and the joint will separate. The chain separates
* easily and does not recover. I used a bias factor of 0.2. A larger value lead to
* the bridge collapsing when a heavy cube drops on it.
*
* Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
* Velocities, but in other ways it is worse. The bridge and chain are much more
* stable, but the simple pendulum goes unstable at high angular velocities.
*
* Full NGS - stable in all tests. The joints display good stiffness. The bridge
* still sags, but this is better than infinite forces.
*
* Recommendations
* Pseudo Velocities are not really worthwhile because the bridge and chain cannot
* recover from joint separation. In other cases the benefit over Baumgarte is small.
*
* Modified NGS is not a robust method for the revolute joint due to the violent
* instability seen in the simple pendulum. Perhaps it is viable with other constraint
* types, especially scalar constraints where the effective mass is a scalar.
*
* This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
* and is very fast. I don't think we can escape Baumgarte, especially in highly
* demanding cases where high constraint fidelity is not needed.
*
* Full NGS is robust and easy on the eyes. I recommend this as an option for
* higher fidelity simulation and certainly for suspension bridges and long chains.
* Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
* joint separation can be problematic. The number of NGS iterations can be reduced
* for better performance without harming robustness much.
*
* Each joint in a can be handled differently in the position solver. So I recommend
* a system where the user can select the algorithm on a per joint basis. I would
* probably default to the slower Full NGS and let the user select the faster
* Baumgarte method in performance critical scenarios.
*/
/*
* Cache Performance
*
* The Box2D solvers are dominated by cache misses. Data structures are designed
* to increase the number of cache hits. Much of misses are due to random access
* to body data. The constraint structures are iterated over linearly, which leads
* to few cache misses.
*
* The bodies are not accessed during iteration. Instead read only data, such as
* the mass values are stored with the constraints. The mutable data are the constraint
* impulses and the bodies velocities/positions. The impulses are held inside the
* constraint structures. The body velocities/positions are held in compact, temporary
* arrays to increase the number of cache hits. Linear and angular velocity are
* stored in a single array since multiple arrays lead to multiple misses.
*/
/*
* 2D Rotation
*
* R = [cos(theta) -sin(theta)]
* [sin(theta) cos(theta) ]
*
* thetaDot = omega
*
* Let q1 = cos(theta), q2 = sin(theta).
* R = [q1 -q2]
* [q2 q1]
*
* q1Dot = -thetaDot * q2
* q2Dot = thetaDot * q1
*
* q1_new = q1_old - dt * w * q2
* q2_new = q2_old + dt * w * q1
* then normalize.
*
* This might be faster than computing sin+cos.
* However, we can compute sin+cos of the same angle fast.
*/
public b2Island(int bodyCapacity, int contactCapacity, int jointCapacity, b2ContactListener listener)
{
m_bodyCapacity = bodyCapacity;
m_contactCapacity = contactCapacity;
m_jointCapacity = jointCapacity;
m_bodyCount = 0;
m_contactCount = 0;
m_jointCount = 0;
m_listener = listener;
m_bodies = new b2Body[bodyCapacity];
m_contacts = new b2Contact[contactCapacity];
m_joints = new b2Joint[jointCapacity];
m_velocities = Arrays.InitializeWithDefaultInstances <b2Velocity>(m_bodyCapacity);
m_positions = Arrays.InitializeWithDefaultInstances <b2Position>(m_bodyCapacity);
}
public void DrawJoint(b2Joint joint)
{
b2Body bodyA = joint.GetBodyA();
b2Body bodyB = joint.GetBodyB();
b2Transform xf1 = bodyA.Transform;
b2Transform xf2 = bodyB.Transform;
b2Vec2 x1 = xf1.p;
b2Vec2 x2 = xf2.p;
b2Vec2 p1 = joint.GetAnchorA();
b2Vec2 p2 = joint.GetAnchorB();
b2Color color = new b2Color(0.5f, 0.8f, 0.8f);
switch (joint.JointType)
{
case b2JointType.e_distanceJoint:
m_debugDraw.DrawSegment(p1, p2, color);
break;
case b2JointType.e_pulleyJoint:
{
b2PulleyJoint pulley = (b2PulleyJoint)joint;
b2Vec2 s1 = pulley.GetGroundAnchorA();
b2Vec2 s2 = pulley.GetGroundAnchorB();
m_debugDraw.DrawSegment(s1, p1, color);
m_debugDraw.DrawSegment(s2, p2, color);
m_debugDraw.DrawSegment(s1, s2, color);
}
break;
case b2JointType.e_mouseJoint:
// don't draw this
break;
default:
m_debugDraw.DrawSegment(x1, p1, color);
m_debugDraw.DrawSegment(p1, p2, color);
m_debugDraw.DrawSegment(x2, p2, color);
}
}
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;
}
internal static void Destroy(b2Joint joint)
{
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Joint obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public void DestroyJoint(b2Joint j)
{
if (IsLocked())
{
return;
}
bool collideConnected = j.m_collideConnected;
// Remove from the doubly linked list.
if (j.Prev)
{
j.Prev.Next = j.Next;
}
if (j.Next)
{
j.Next.Prev = j.Prev;
}
if (j == m_jointList)
{
m_jointList = j.Next;
}
// Disconnect from island graph.
b2Body bodyA = j.m_bodyA;
b2Body bodyB = j.m_bodyB;
// Wake up connected bodies.
bodyA.SetAwake(true);
bodyB.SetAwake(true);
// Remove from body 1.
if (j.m_edgeA.prev)
{
j.m_edgeA.prev.next = j.m_edgeA.next;
}
if (j.m_edgeA.next)
{
j.m_edgeA.next.prev = j.m_edgeA.prev;
}
if (j.m_edgeA == bodyA.JointList)
{
bodyA.JointList = j.m_edgeA.next;
}
j.m_edgeA.prev = null;
j.m_edgeA.next = null;
// Remove from body 2
if (j.m_edgeB.prev)
{
j.m_edgeB.prev.next = j.m_edgeB.next;
}
if (j.m_edgeB.next)
{
j.m_edgeB.next.prev = j.m_edgeB.prev;
}
if (j.m_edgeB == bodyB.JointList)
{
bodyB.JointList = j.m_edgeB.next;
}
j.m_edgeB.prev = null;
j.m_edgeB.next = null;
b2Joint.Destroy(j);
--m_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
b2ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.FlagForFiltering();
}
edge = edge.next;
}
}
}
internal static b2Joint Create(b2JointDef def)
{
b2Joint joint = null;
switch (def.type)
{
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;
case b2JointType.e_motorJoint:
{
joint = new b2MotorJoint((b2MotorJointDef)def);
}
break;
default:
Debug.Assert(false);
break;
}
return(joint);
}
public b2Joint CreateJoint(b2JointDef def)
{
if (IsLocked())
{
return null;
}
b2Joint j = b2Joint.Create(def);
// Connect to the world list.
j.Prev = null;
j.Next = m_jointList;
if (m_jointList)
{
m_jointList.Prev = j;
}
m_jointList = j;
++m_jointCount;
// Connect to the bodies' doubly linked lists.
j.m_edgeA.joint = j;
j.m_edgeA.other = j.m_bodyB;
j.m_edgeA.prev = null;
j.m_edgeA.next = j.m_bodyA.JointList;
if (j.m_bodyA.JointList) j.m_bodyA.JointList.prev = &j.m_edgeA;
j.m_bodyA.JointList = &j.m_edgeA;
j.m_edgeB.joint = j;
j.m_edgeB.other = j.m_bodyA;
j.m_edgeB.prev = null;
j.m_edgeB.next = j.m_bodyB.JointList;
if (j.m_bodyB.JointList) j.m_bodyB.JointList.prev = &j.m_edgeB;
j.m_bodyB.JointList = &j.m_edgeB;
b2Body bodyA = def.bodyA;
b2Body bodyB = def.bodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (def.collideConnected == false)
{
b2ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.FlagForFiltering();
}
edge = edge.next;
}
}
// Note: creating a joint doesn't wake the bodies.
return j;
}
public virtual void JointDestroyed(b2Joint joint)
{
}
// Find islands, integrate and solveraints, solve positionraints
public void Solve(b2TimeStep step)
{
m_profile.solveInit = 0.0f;
m_profile.solveVelocity = 0.0f;
m_profile.solvePosition = 0.0f;
// Size the island for the worst case.
b2Island island = new b2Island(m_bodyCount,
m_contactManager.ContactCount,
m_jointCount,
m_contactManager.ContactListener);
// Clear all the island flags.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
c.ContactFlags &= ~b2ContactFlags.e_islandFlag;
}
for (b2Joint j = m_jointList; j; j = j.Next)
{
j.m_islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = m_bodyCount;
b2Body[] stack = new b2Body[stackSize];
for (b2Body seed = m_bodyList; seed != null; seed = seed.Next)
{
if (seed.BodyFlags & b2BodyFlags.e_islandFlag)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Reset island and stack.
island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed.BodyFlags |= b2BodyFlags.e_islandFlag;
// Perform a depth first search (DFS) on theraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
b2Body b = stack[--stackCount];
island.Add(b);
// Make sure the body is awake.
b.SetAwake(true);
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
// Search all contacts connected to this body.
for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next)
{
b2Contact contact = ce.contact;
// Has this contact already been added to an island?
if (contact.ContactFlags & b2ContactFlags.e_islandFlag)
{
continue;
}
// Is this contact solid and touching?
if (contact.IsEnabled() == false ||
contact.IsTouching() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
island.Add(contact);
contact.ContactFlags |= b2ContactType.e_islandFlag;
b2Body other = ce.other;
// Was the other body already added to this island?
if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
// Search all joints connect to this body.
for (b2JointEdge je = b.JointList; je; je = je.next)
{
if (je.joint.IslandFlag == true)
{
continue;
}
b2Body other = je.other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
island.Add(je.joint);
je.joint.m_islandFlag = true;
if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0)
{
continue;
}
stack[stackCount++] = other;
other.BodyFlags |= b2BodyFlags.e_islandFlag;
}
}
b2Profile profile = island.Solve(step, m_gravity, m_allowSleep);
m_profile.solveInit += profile.solveInit;
m_profile.solveVelocity += profile.solveVelocity;
m_profile.solvePosition += profile.solvePosition;
// Post solve cleanup.
for (int i = 0; i < island.m_bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
b2Body b = island.m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
b.BodyFlags &= ~b2BodyFlags.e_islandFlag;
}
}
}
{
b2Timer timer;
// Synchronize fixtures, check for out of range bodies.
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
// If a body was not in an island then it did not move.
if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0)
{
continue;
}
if (b.GetBodyType() == b2BodyType.b2_staticBody)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
m_contactManager.FindNewContacts();
m_profile.broadphase = timer.GetMilliseconds();
}
}
public Gears()
{
b2Body ground = null;
{
b2BodyDef bd = new b2BodyDef();
ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(50.0f, 0.0f), new b2Vec2(-50.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
// Gears co
{
b2CircleShape circle1 = new b2CircleShape();
circle1.Radius = 1.0f;
b2PolygonShape box = new b2PolygonShape();
box.SetAsBox(0.5f, 5.0f);
b2CircleShape circle2 = new b2CircleShape();
circle2.Radius = 2.0f;
b2BodyDef bd1 = new b2BodyDef();
bd1.type = b2BodyType.b2_staticBody;
bd1.position.Set(10.0f, 9.0f);
b2Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1, 0.0f);
b2BodyDef bd2 = new b2BodyDef();
bd2.type = b2BodyType.b2_dynamicBody;
bd2.position.Set(10.0f, 8.0f);
b2Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(box, 5.0f);
b2BodyDef bd3 = new b2BodyDef();
bd3.type = b2BodyType.b2_dynamicBody;
bd3.position.Set(10.0f, 6.0f);
b2Body body3 = m_world.CreateBody(bd3);
body3.CreateFixture(circle2, 5.0f);
b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
jd1.Initialize(body2, body1, bd1.position);
b2Joint joint1 = m_world.CreateJoint(jd1);
b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
jd2.Initialize(body2, body3, bd3.position);
b2Joint joint2 = m_world.CreateJoint(jd2);
b2GearJointDef jd4 = new b2GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body3;
jd4.joint1 = joint1;
jd4.joint2 = joint2;
jd4.ratio = circle2.Radius / circle1.Radius;
m_world.CreateJoint(jd4);
}
{
b2CircleShape circle1 = new b2CircleShape();
circle1.Radius = 1.0f;
b2CircleShape circle2 = new b2CircleShape();
circle2.Radius = 2.0f;
b2PolygonShape box = new b2PolygonShape();
box.SetAsBox(0.5f, 5.0f);
b2BodyDef bd1 = new b2BodyDef();
bd1.type = b2BodyType.b2_dynamicBody;
bd1.position.Set(-3.0f, 12.0f);
b2Body body1 = m_world.CreateBody(bd1);
body1.CreateFixture(circle1, 5.0f);
b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
jd1.BodyA = ground;
jd1.BodyB = body1;
jd1.localAnchorA = ground.GetLocalPoint(bd1.position);
jd1.localAnchorB = body1.GetLocalPoint(bd1.position);
jd1.referenceAngle = body1.Angle - ground.Angle;
m_joint1 = (b2RevoluteJoint)m_world.CreateJoint(jd1);
b2BodyDef bd2 = new b2BodyDef();
bd2.type = b2BodyType.b2_dynamicBody;
bd2.position.Set(0.0f, 12.0f);
b2Body body2 = m_world.CreateBody(bd2);
body2.CreateFixture(circle2, 5.0f);
b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
jd2.Initialize(ground, body2, bd2.position);
m_joint2 = (b2RevoluteJoint)m_world.CreateJoint(jd2);
b2BodyDef bd3 = new b2BodyDef();
bd3.type = b2BodyType.b2_dynamicBody;
bd3.position.Set(2.5f, 12.0f);
b2Body body3 = m_world.CreateBody(bd3);
body3.CreateFixture(box, 5.0f);
b2PrismaticJointDef jd3 = new b2PrismaticJointDef();
jd3.Initialize(ground, body3, bd3.position, new b2Vec2(0.0f, 1.0f));
jd3.lowerTranslation = -5.0f;
jd3.upperTranslation = 5.0f;
jd3.enableLimit = true;
m_joint3 = (b2PrismaticJoint)m_world.CreateJoint(jd3);
b2GearJointDef jd4 = new b2GearJointDef();
jd4.BodyA = body1;
jd4.BodyB = body2;
jd4.joint1 = m_joint1;
jd4.joint2 = m_joint2;
jd4.ratio = circle2.Radius / circle1.Radius;
m_joint4 = (b2GearJoint)m_world.CreateJoint(jd4);
b2GearJointDef jd5 = new b2GearJointDef();
jd5.BodyA = body2;
jd5.BodyB = body3;
jd5.joint1 = m_joint2;
jd5.joint2 = m_joint3;
jd5.ratio = -1.0f / circle2.Radius;
m_joint5 = (b2GearJoint)m_world.CreateJoint(jd5);
}
}
private void DrawJoint(b2Joint joint);
private void DrawJoint(b2Joint joint);
/// 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 void DestroyJoint(b2Joint joint)
{
Box2dPINVOKE.b2World_DestroyJoint(swigCPtr, b2Joint.getCPtr(joint));
}
public virtual void SayGoodbye(b2Joint joint)
{
Box2dPINVOKE.b2DestructionListener_SayGoodbye__SWIG_0(swigCPtr, b2Joint.getCPtr(joint));
}
public void DrawDebugData()
{
if (m_debugDraw == null)
{
return;
}
b2DrawFlags flags = m_debugDraw.GetFlags();
if (flags & b2DrawFlags.e_shapeBit)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b2Transform xf = b.Transform;
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == b2BodyType.b2_staticBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == b2BodyType.b2_kinematicBody)
{
DrawShape(f, xf, new b2Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new b2Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new b2Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_jointBit))
{
for (b2Joint j = m_jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if (flags.HasFlag(b2DrawFlags.e_pairBit))
{
b2Color color = new b2Color(0.3f, 0.9f, 0.9f);
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
//b2Fixture fixtureA = c.GetFixtureA();
//b2Fixture fixtureB = c.GetFixtureB();
//b2Vec2 cA = fixtureA.GetAABB().GetCenter();
//b2Vec2 cB = fixtureB.GetAABB().GetCenter();
//m_debugDraw.DrawSegment(cA, cB, color);
}
}
if (flags.HasFlag(b2DrawFlags.e_aabbBit))
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase bp = m_contactManager.BroadPhase;
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
if (b.IsActive() == false)
{
continue;
}
for (b2Fixture f = b.FixtureList; f != null; f = f.Next)
{
for (int i = 0; i < f.ProxyCount; ++i)
{
b2FixtureProxy proxy = f.Proxies[i];
b2AABB aabb = bp.GetFatAABB(proxy.proxyId);
b2Vec2[] vs = new b2Vec2[4];
vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw.DrawPolygon(vs, 4, color);
}
}
}
}
if (flags.HasFlag(b2DrawFlags.e_centerOfMassBit))
{
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b2Transform xf = b.Transform;
xf.p = b.WorldCenter;
m_debugDraw.DrawTransform(xf);
}
}
}
public void Add(b2Joint joint)
{
Debug.Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount++] = joint;
}
public b2Joint CreateJoint(b2JointDef def)
{
if (IsLocked())
{
return(null);
}
b2Joint j = b2Joint.Create(def);
// Connect to the world list.
j.Prev = null;
j.Next = m_jointList;
if (m_jointList)
{
m_jointList.Prev = j;
}
m_jointList = j;
++m_jointCount;
// Connect to the bodies' doubly linked lists.
j.m_edgeA.joint = j;
j.m_edgeA.other = j.m_bodyB;
j.m_edgeA.prev = null;
j.m_edgeA.next = j.m_bodyA.JointList;
if (j.m_bodyA.JointList)
{
j.m_bodyA.JointList.prev = &j.m_edgeA;
}
j.m_bodyA.JointList = &j.m_edgeA;
j.m_edgeB.joint = j;
j.m_edgeB.other = j.m_bodyA;
j.m_edgeB.prev = null;
j.m_edgeB.next = j.m_bodyB.JointList;
if (j.m_bodyB.JointList)
{
j.m_bodyB.JointList.prev = &j.m_edgeB;
}
j.m_bodyB.JointList = &j.m_edgeB;
b2Body bodyA = def.bodyA;
b2Body bodyB = def.bodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (def.collideConnected == false)
{
b2ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.FlagForFiltering();
}
edge = edge.next;
}
}
// Note: creating a joint doesn't wake the bodies.
return(j);
}
public void AddJoint(b2Joint joint)
{
//b2Settings.b2Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount++] = joint;
}
public void DestroyJoint(b2Joint j)
{
if (IsLocked())
{
return;
}
bool collideConnected = j.m_collideConnected;
// Remove from the doubly linked list.
if (j.Prev)
{
j.Prev.Next = j.Next;
}
if (j.Next)
{
j.Next.Prev = j.Prev;
}
if (j == m_jointList)
{
m_jointList = j.Next;
}
// Disconnect from island graph.
b2Body bodyA = j.m_bodyA;
b2Body bodyB = j.m_bodyB;
// Wake up connected bodies.
bodyA.SetAwake(true);
bodyB.SetAwake(true);
// Remove from body 1.
if (j.m_edgeA.prev)
{
j.m_edgeA.prev.next = j.m_edgeA.next;
}
if (j.m_edgeA.next)
{
j.m_edgeA.next.prev = j.m_edgeA.prev;
}
if (j.m_edgeA == bodyA.JointList)
{
bodyA.JointList = j.m_edgeA.next;
}
j.m_edgeA.prev = null;
j.m_edgeA.next = null;
// Remove from body 2
if (j.m_edgeB.prev)
{
j.m_edgeB.prev.next = j.m_edgeB.next;
}
if (j.m_edgeB.next)
{
j.m_edgeB.next.prev = j.m_edgeB.prev;
}
if (j.m_edgeB == bodyB.JointList)
{
bodyB.JointList = j.m_edgeB.next;
}
j.m_edgeB.prev = null;
j.m_edgeB.next = null;
b2Joint.Destroy(j);
--m_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
b2ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.FlagForFiltering();
}
edge = edge.next;
}
}
}
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
};
}
/// 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 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);
}
}
/**
* Called when any joint is about to be destroyed due
* to the destruction of one of its attached bodies.
*/
public void SayGoodbyeJoint(b2Joint joint)
{
}
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;
}