public void Create(b2Body body, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
Next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
for (int i = 0; i < childCount; ++i)
{
b2FixtureProxy proxy = new b2FixtureProxy();
proxy.fixture = null;
proxy.proxyId = b2BroadPhase.e_nullProxy;
m_proxies.Add(proxy);
}
m_proxyCount = 0;
m_density = def.density;
}
public void Add(b2Body body)
{
Debug.Assert(m_bodyCount < m_bodyCapacity);
body.IslandIndex = m_bodyCount;
m_bodies[m_bodyCount] = body;
++m_bodyCount;
}
public MouseTouch(b2World world, RubeBasicLayer layer)
{
parent = layer;
m_world = world;
b2BodyDef bodyDef = new b2BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
}
public JumpPad(b2Vec2 JumpImpuls, CCPoint Position, b2World gameWorld)
{
this.Texture = new CCTexture2D ("jumppad");
this.Scale = SpriteScale;
this.Position = Position;
this.IsAntialiased = false;
jumpImpuls = JumpImpuls;
totalJumps = 0;
//box2d
b2BodyDef jumpPadDef = new b2BodyDef ();
jumpPadDef.type = b2BodyType.b2_kinematicBody;
jumpPadDef.position = new b2Vec2 ((Position.X + this.ScaledContentSize.Width/2)/PhysicsHandler.pixelPerMeter, (Position.Y + this.ScaledContentSize.Height/4) / PhysicsHandler.pixelPerMeter);
JumpPadBody = gameWorld.CreateBody (jumpPadDef);
b2PolygonShape jumpPadShape = new b2PolygonShape ();
jumpPadShape.SetAsBox ((float)this.ScaledContentSize.Width / PhysicsHandler.pixelPerMeter / 2, (float)this.ScaledContentSize.Height / PhysicsHandler.pixelPerMeter / 4);// /4 weil die hitbox nur die hälfte der textur ist
b2FixtureDef jumpPadFixture = new b2FixtureDef ();
jumpPadFixture.shape = jumpPadShape;
jumpPadFixture.density = 0.0f; //Dichte
jumpPadFixture.restitution = 0f; //Rückprall
jumpPadFixture.friction = 0f;
jumpPadFixture.userData = WorldFixtureData.jumppad;
JumpPadBody.CreateFixture (jumpPadFixture);
//
}
public void Create(b2Body body, b2FixtureDef def)
{
UserData = def.userData;
Friction = def.friction;
Restitution = def.restitution;
Body = body;
Next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
Shape = def.shape.Clone();
// Reserve proxy space
int childCount = Shape.GetChildCount();
m_proxies = b2ArrayPool <b2FixtureProxy> .Create(childCount, true);
for (int i = 0; i < childCount; ++i)
{
m_proxies[i].fixture = null;
m_proxies[i].proxyId = b2BroadPhase.e_nullProxy;
}
m_proxyCount = 0;
Density = def.density;
}
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 void RemoveBody()
{
if (body != null)
{
body.World.DestroyBody(body);
body = null;
}
}
public void ClearForces()
{
for (b2Body body = m_bodyList; body != null; body = body.Next)
{
body.Force = new b2Vec2();
body.Torque = 0.0f;
}
}
public WallJumpManager(b2Body parentJumpBody, float JumpTimeNeeded, float JumpHeight, float JumpWidth)
{
jumpBody = parentJumpBody;
maxTime = JumpTimeNeeded;
jumpSize = new CCSize (JumpWidth, JumpHeight);
CurrentJumpingDirection = Direction.Left;
}
public void Initialize(b2Body b1, b2Body b2,
b2Vec2 anchor1, b2Vec2 anchor2)
{
BodyA = b1;
BodyB = b2;
localAnchorA = BodyA.GetLocalPoint(anchor1);
localAnchorB = BodyB.GetLocalPoint(anchor2);
b2Vec2 d = anchor2 - anchor1;
length = d.Length;
}
public int CompareTo(object obj)
{
b2Body b2 = obj as b2Body;
if (b2 == null)
{
return(-1);
}
if (BodyType != b2.BodyType)
{
return(BodyType.CompareTo(b2.BodyType));
}
return(obj == this ? 0 : -1);
}
//
public void SetAllowSleeping(bool flag)
{
if (flag == m_allowSleep)
{
return;
}
m_allowSleep = flag;
if (m_allowSleep == false)
{
for (b2Body b = m_bodyList; b != null; b = b.Next)
{
b.SetAwake(true);
}
}
}
/**
* the destructor cannot access the allocator (no destructor arguments allowed by C++).
* We need separation create/destroy functions from the constructor/destructor because
*/
public void Create(b2Body body, b2Transform xf, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
m_next = null;
m_filter = def.filter.Copy();
m_isSensor = def.isSensor;
m_shape = def.shape.Copy();
m_density = def.density;
}
protected void CreateBodyWithSpriteAndFixture(b2World world, b2BodyDef bodyDef,
b2FixtureDef fixtureDef, string spriteName)
{
// this is the meat of our class, it creates (OR recreates) the body in the world with the body definition, fixture definition and sprite name
RemoveBody(); //if remove the body if it already exists
RemoveSprite(); //if remove the sprite if it already exists
sprite = new CCSprite(spriteName);
AddChild(sprite);
body = world.CreateBody(bodyDef);
body.UserData = this;
if (fixtureDef != null)
body.CreateFixture(fixtureDef);
}
public Platform(List<CCPoint> platformWaypoints, int platformSpeed, Container gameContainer)
{
this.Texture = new CCTexture2D("platform");
this.Scale = SpriteScale;
this.IsAntialiased = false;
this.Position = platformWaypoints [0];
Waypoints = platformWaypoints;
speed = platformSpeed;
//umso geringer der speed umso schneller die platform
CurrentWaypoint = 0;
wayToMove = new CCSize (Waypoints [CurrentWaypoint + 1].X - Waypoints [CurrentWaypoint].X, Waypoints [CurrentWaypoint + 1].Y - Waypoints [CurrentWaypoint].Y);
//box2d
b2BodyDef platformDef = new b2BodyDef ();
platformDef.type = b2BodyType.b2_kinematicBody;
platformDef.position = new b2Vec2 (Waypoints[CurrentWaypoint].X / PhysicsHandler.pixelPerMeter, Waypoints[CurrentWaypoint].Y / PhysicsHandler.pixelPerMeter);
platformBody = gameContainer.physicsHandler.gameWorld.CreateBody (platformDef);
b2PolygonShape platformShape = new b2PolygonShape ();
platformShape.SetAsBox ((float)this.ScaledContentSize.Width / PhysicsHandler.pixelPerMeter / 2, (float)this.ScaledContentSize.Height / PhysicsHandler.pixelPerMeter / 2);
b2FixtureDef platformFixture = new b2FixtureDef ();
platformFixture.shape = platformShape;
platformFixture.density = 0.0f; //Dichte
platformFixture.restitution = 0f; //Rückprall
platformFixture.userData = WorldFixtureData.platform;
platformBody.CreateFixture (platformFixture);
//
this.Position = new CCPoint (platformBody.Position.x * PhysicsHandler.pixelPerMeter, platformBody.Position.y * PhysicsHandler.pixelPerMeter);
progressionX = wayToMove.Width/ (float)speed;
progressionY = wayToMove.Height/(float)speed ;
if (float.IsInfinity (progressionX))
progressionX = 0;
if (float.IsInfinity (progressionY))
progressionY = 0;
b2Vec2 Velocity = platformBody.LinearVelocity;
Velocity.y = progressionY;
Velocity.x = progressionX;
platformBody.LinearVelocity = Velocity;
}
public void SetJson(string fullpath)
{
Console.WriteLine("Full path is: %s", fullpath);
Nb2dJson json = new Nb2dJson();
StringBuilder tmp = new StringBuilder();
m_world = json.ReadFromFile(fullpath, tmp);
if (m_world != null)
{
Console.WriteLine("Loaded JSON ok");
m_world.SetDebugDraw(m_debugDraw);
b2BodyDef bodyDef = new b2BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
}
else
Console.WriteLine(tmp); //if this warning bothers you, turn off "Typecheck calls to printf/scanf" in the project build settings
}
public b2Body CreateBody(b2BodyDef def)
{
if (IsLocked())
{
return(null);
}
b2Body b = new b2Body(def, this);
// Add to world doubly linked list.
b.Prev = null;
b.Next = m_bodyList;
if (m_bodyList != null)
{
m_bodyList.Prev = b;
}
m_bodyList = b;
++m_bodyCount;
return(b);
}
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 virtual bool ShouldCollide(b2Body other)
{
// At least one body should be dynamic.
if (m_type != b2_dynamicBody && other.m_type != b2_dynamicBody)
{
return(false);
}
// Does a joint prevent collision?
for (b2JointEdge *jn = m_jointList; jn; jn = jn.next)
{
if (jn.other == other)
{
if (jn.joint.m_collideConnected == false)
{
return(false);
}
}
}
return(true);
}
public virtual bool ShouldCollide(b2Body other)
{
// At least one body should be dynamic.
if (m_type != b2BodyType.b2_dynamicBody && other.m_type != b2BodyType.b2_dynamicBody)
{
return(false);
}
// Does a joint prevent collision?
for (b2JointEdge jn = m_jointList; jn != null; jn = jn.Next)
{
if (jn.Other == other)
{
if (jn.Joint.GetCollideConnected() == false)
{
return(false);
}
}
}
return(true);
}
public BoxProp(
b2World b2world,
double[] size,
double[] position
)
{
/*
static rectangle shaped prop
pars:
size - array [width, height]
position - array [x, y], in world meters, of center
*/
this.size = size;
//initialize body
var bdef = new b2BodyDef();
bdef.position = new b2Vec2(position[0], position[1]);
bdef.angle = 0;
bdef.fixedRotation = true;
this.body = b2world.CreateBody(bdef);
//initialize shape
var fixdef = new b2FixtureDef();
var shape = new b2PolygonShape();
fixdef.shape = shape;
shape.SetAsBox(this.size[0] / 2, this.size[1] / 2);
fixdef.restitution = 0.4; //positively bouncy!
this.body.CreateFixture(fixdef);
}
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;
}
public void AddPair(ref b2FixtureProxy proxyA, ref b2FixtureProxy proxyB)
{
b2Fixture fixtureA = proxyA.fixture;
b2Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
b2Body bodyA = fixtureA.Body;
b2Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
b2ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
b2Fixture fA = edge.Contact.FixtureA;
b2Fixture fB = edge.Contact.FixtureB;
int iA = edge.Contact.m_indexA;
int iB = edge.Contact.m_indexB;
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.Next;
}
// Does a joint override collision? Is at least one body dynamic?
if (!bodyB.ShouldCollide(bodyA))
{
return;
}
// Check user filtering.
if (m_contactFilter != null && !m_contactFilter.ShouldCollide(fixtureA, fixtureB))
{
return;
}
// Call the factory.
b2Contact c = b2Contact.Create(fixtureA, indexA, fixtureB, indexB);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
//fixtureA = c.FixtureA;
//fixtureB = c.FixtureB;
//indexA = c.m_indexA;
//indexB = c.m_indexB;
bodyA = c.FixtureA.Body;
bodyB = c.FixtureB.Body;
// Insert into the world.
c.Prev = null;
c.Next = m_contactList;
if (m_contactList != null)
{
m_contactList.Prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c.NodeA.Contact = c;
c.NodeA.Other = bodyB;
c.NodeA.Prev = null;
c.NodeA.Next = bodyA.ContactList;
if (bodyA.ContactList != null)
{
bodyA.ContactList.Prev = c.NodeA;
}
bodyA.ContactList = c.NodeA;
// Connect to body B
c.NodeB.Contact = c;
c.NodeB.Other = bodyA;
c.NodeB.Prev = null;
c.NodeB.Next = bodyB.ContactList;
if (bodyB.ContactList != null)
{
bodyB.ContactList.Prev = c.NodeB;
}
bodyB.ContactList = c.NodeB;
// Wake up the bodies
bodyA.SetAwake(true);
bodyB.SetAwake(true);
++m_contactCount;
}
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);
}
}
}
/// <summary>
/// ��һ���ڳ��������һ����̬��bird����
/// �ڶ�����bird�����ж���
/// ����������box-2d����������bird�������������
/// </summary>
private void AddBird()
{
CCSprite bird = CCSprite.spriteWithFile("imgs/bird/bird_01");
bird.rotation = -15;
// bird���ж���֡����
List<CCSpriteFrame> frames = new List<CCSpriteFrame>();
for (int i = 1; i < 3; i++)
{
// ֡��ͼ
CCTexture2D texture = CCTextureCache.sharedTextureCache().addImage("imgs/bird/bird_0" + i);
// �������һ�������bug����������õĻ����ͻᲥ�Ų���������
texture.Name = (uint)i;
var frame = CCSpriteFrame.frameWithTexture(texture, new CCRect(0, 0, texture.ContentSizeInPixels.width, texture.ContentSizeInPixels.height));
frames.Add(frame);
}
// �����
CCAnimation marmotShowanimation = CCAnimation.animationWithFrames(frames, 0.1f);
CCAnimate flyAction = CCAnimate.actionWithAnimation(marmotShowanimation, false);
flyRepeatAction = CCRepeat.actionWithAction(flyAction, 2);
flyRepeatAction.tag = 0;
bird.runAction(flyRepeatAction);
// �����������ж���һ��body��������λ�ã�����bird��֮��Ӧ
b2BodyDef ballBodyDef = new b2BodyDef();
ballBodyDef.type = b2BodyType.b2_dynamicBody;
ballBodyDef.position = new b2Vec2(AppDelegate.screenSize.width / PTM_RATIO / 2, (float)(AppDelegate.screenSize.height / PTM_RATIO));
ballBodyDef.userData = bird;
birdBody = world.CreateBody(ballBodyDef);
// Ϊbody������״��������һЩ��������
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(bird.contentSize.width / 2 / PTM_RATIO, bird.contentSize.height / 2 / PTM_RATIO);
b2FixtureDef fixtureDef = new b2FixtureDef();
fixtureDef.shape = shape;
fixtureDef.density = 500.0f;
fixtureDef.friction = 0.5f;
birdBody.CreateFixture(fixtureDef);
this.addChild(bird);
}
public void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < m_bodyCount);
Debug.Assert(toiIndexB < m_bodyCount);
// Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
m_positions[i].c = b.Sweep.c;
m_positions[i].a = b.Sweep.a;
m_velocities[i].v = b.LinearVelocity;
m_velocities[i].w = b.AngularVelocity;
}
b2ContactSolverDef contactSolverDef;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.step = subStep;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
// Solve position constraints.
for (int i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
#if false
// Is the new position really safe?
for (int i = 0; i < m_contactCount; ++i)
{
b2Contact c = m_contacts[i];
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
b2DistanceInput input = new b2DistanceInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.transformA = bA.Transform;
input.transformB = bB.Transform;
input.useRadii = false;
b2DistanceOutput output;
b2SimplexCache cache = new b2SimplexCache();
cache.count = 0;
output = b2Distance(cache, input);
if (output.distance == 0 || cache.count == 3)
{
cache.count += 0;
}
}
#endif
// Leap of faith to new safe state.
m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c;
m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a;
m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c;
m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length;
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
// Sync bodies
b2Body body = m_bodies[i];
body.Sweep.c = c;
body.Sweep.a = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(contactSolver.m_velocityConstraints);
}
public void SetBodyName(b2Body body, String name)
{
m_bodyToNameMap.Add(body, name);
}
// Find TOI contacts and solve them.
public void SolveTOI(b2TimeStep step)
{
b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener);
if (m_stepComplete)
{
for (b2Body b = m_bodyList; b; b = b.Next)
{
b.BodyFlags &= ~b2Body.e_islandFlag;
b.m_sweep.alpha0 = 0.0f;
}
for (b2Contact c = m_contactManager.ContactList; c; c = c.Next)
{
// Invalidate TOI
c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag);
c.m_toiCount = 0;
c.m_toi = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
b2Contact minContact = null;
float minAlpha = 1.0f;
for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next)
{
// Is this contact disabled?
if (c.IsEnabled() == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.m_toiCount > b2Settings.b2_maxSubSteps)
{
continue;
}
float alpha = 1.0f;
if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag))
{
// This contact has a valid cached TOI.
alpha = c.m_toi;
}
else
{
b2Fixture fA = c.GetFixtureA();
b2Fixture fB = c.GetFixtureB();
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2BodyType typeA = bA.BodyType;
b2BodyType typeB = bB.BodyType;
bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody;
bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody;
// Is at least one body active (awake and dynamic or kinematic)?
if (activeA == false && activeB == false)
{
continue;
}
bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody;
bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody;
// Are these two non-bullet dynamic bodies?
if (collideA == false && collideB == false)
{
continue;
}
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
float alpha0 = bA.Sweep.alpha0;
if (bA.Sweep.alpha0 < bB.Sweep.alpha0)
{
alpha0 = bB.Sweep.alpha0;
bA.Sweep.Advance(alpha0);
}
else if (bB.Sweep.alpha0 < bA.Sweep.alpha0)
{
alpha0 = bA.Sweep.alpha0;
bB.Sweep.Advance(alpha0);
}
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
// Compute the time of impact in interval [0, minTOI]
b2TOIInput input = new b2TOIInput();
input.proxyA.Set(fA.Shape, indexA);
input.proxyB.Set(fB.Shape, indexB);
input.sweepA = bA.Sweep;
input.sweepB = bB.Sweep;
input.tMax = 1.0f;
b2TOIOutput output = b2TimeOfImpact(input);
// Beta is the fraction of the remaining portion of the .
float beta = output.t;
if (output.state == b2TOIOutputType.e_touching)
{
alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.m_toi = alpha;
c.ContactFlags |= b2ContactFlags.e_toiFlag;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha)
{
// No more TOI events. Done!
m_stepComplete = true;
break;
}
{
// Advance the bodies to the TOI.
b2Fixture fA = minContact.GetFixtureA();
b2Fixture fB = minContact.GetFixtureB();
b2Body bA = fA.Body;
b2Body bB = fB.Body;
b2Sweep backup1 = bA.Sweep;
b2Sweep backup2 = bB.Sweep;
bA.Advance(minAlpha);
bB.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(m_contactManager.ContactListener);
minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag;
++minContact.m_toiCount;
// Is the contact solid?
if (minContact.IsEnabled() == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.SetEnabled(false);
bA.Sweep = backup1;
bB.Sweep = backup2;
bA.SynchronizeTransform();
bB.SynchronizeTransform();
continue;
}
bA.SetAwake(true);
bB.SetAwake(true);
// Build the island
island.Clear();
island.Add(bA);
island.Add(bB);
island.Add(minContact);
bA.BodyFlags |= b2BodyFlags.e_islandFlag;
bB.BodyFlags |= b2BodyFlags.e_islandFlag;
minContact.ContentType |= b2ContactFlags.e_islandFlag;
// Get contacts on bodyA and bodyB.
b2Body[] bodies = new b2Body[] { bA, bB };
for (int i = 0; i < 2; ++i)
{
b2Body body = bodies[i];
if (body.BodyType == b2BodyType.b2_dynamicBody)
{
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
if (island.BodyCount == island.BodyCapacity)
{
break;
}
if (island.ContactCount == island.ContactCapacity)
{
break;
}
b2Contact contact = ce.contact;
// Has this contact already been added to the island?
if (contact.ContactType & b2ContactType.e_islandFlag)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
b2Body other = ce.other;
if (other.BodyType == b2BodyType.b2_dynamicBody &&
body.IsBullet() == false && other.IsBullet() == false)
{
continue;
}
// Skip sensors.
bool sensorA = contact.m_fixtureA.m_isSensor;
bool sensorB = contact.m_fixtureB.m_isSensor;
if (sensorA || sensorB)
{
continue;
}
// Tentatively advance the body to the TOI.
b2Sweep backup = other.Sweep;
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(m_contactManager.ContactListener);
// Was the contact disabled by the user?
if (contact.IsEnabled() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Are there contact points?
if (contact.IsTouching() == false)
{
other.Sweep = backup;
other.SynchronizeTransform();
continue;
}
// Add the contact to the island
contact.ContactFlags |= b2ContactFlags.e_islandFlag;
island.Add(contact);
// Has the other body already been added to the island?
if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag))
{
continue;
}
// Add the other body to the island.
other.BodyFlags |= b2BodyFlags.e_islandFlag;
if (other.BodyType != b2BodyType.b2_staticBody)
{
other.SetAwake(true);
}
island.Add(other);
}
}
}
b2TimeStep subStep;
subStep.dt = (1.0f - minAlpha) * step.dt;
subStep.inv_dt = 1.0f / subStep.dt;
subStep.dtRatio = 1.0f;
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < island.m_bodyCount; ++i)
{
b2Body body = island.m_bodies[i];
body.BodyFlags &= ~b2BodyFlags.e_islandFlag;
if (body.BodyType != b2BodyType.b2_dynamicBody)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next)
{
ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
m_contactManager.FindNewContacts();
if (m_subStepping)
{
m_stepComplete = false;
break;
}
}
}
}
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
b2FixtureProxy proxyA = (b2FixtureProxy)proxyUserDataA;
b2FixtureProxy proxyB = (b2FixtureProxy)proxyUserDataB;
b2Fixture fixtureA = proxyA.fixture;
b2Fixture fixtureB = proxyB.fixture;
int indexA = proxyA.childIndex;
int indexB = proxyB.childIndex;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// TODO_ERIN use a hash table to remove a potential bottleneck when both
// bodies have a lot of contacts.
// Does a contact already exist?
b2ContactEdge edge = bodyB.GetContactList();
while (edge)
{
if (edge.other == bodyA)
{
b2Fixture fA = edge.contact.GetFixtureA();
b2Fixture fB = edge.contact.GetFixtureB();
int iA = edge.contact.GetChildIndexA();
int iB = edge.contact.GetChildIndexB();
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
{
// A contact already exists.
return;
}
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
{
// A contact already exists.
return;
}
}
edge = edge.next;
}
// Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
b2Contact c = b2Contact.Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
if (c == null)
{
return;
}
// Contact creation may swap fixtures.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
indexA = c.GetChildIndexA();
indexB = c.GetChildIndexB();
bodyA = fixtureA.GetBody();
bodyB = fixtureB.GetBody();
// Insert into the world.
c.m_prev = null;
c.m_next = m_contactList;
if (m_contactList != null)
{
m_contactList.m_prev = c;
}
m_contactList = c;
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
c.m_nodeA.prev = null;
c.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null)
{
bodyA.m_contactList.prev = &c.m_nodeA;
}
bodyA.m_contactList = &c.m_nodeA;
// Connect to body B
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
c.m_nodeB.prev = null;
c.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null)
{
bodyB.m_contactList.prev = &c.m_nodeB;
}
bodyB.m_contactList = &c.m_nodeB;
// Wake up the bodies
bodyA.SetAwake(true);
bodyB.SetAwake(true);
++m_contactCount;
}
// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
public void Collide()
{
// Update awake contacts.
b2Contact c = m_contactList;
while (c)
{
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
int indexA = c.GetChildIndexA();
int indexB = c.GetChildIndexB();
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
// Is this contact flagged for filtering?
if (c.m_flags & b2Contact.e_filterFlag)
{
// Should these bodies collide?
if (bodyB.ShouldCollide(bodyA) == false)
{
b2Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (m_contactFilter && m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
b2Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c.m_flags &= ~b2Contact.e_filterFlag;
}
bool activeA = bodyA.IsAwake() && bodyA.m_type != b2BodyType.b2_staticBody;
bool activeB = bodyB.IsAwake() && bodyB.m_type != b2BodyType.b2_staticBody;
// At least one body must be awake and it must be dynamic or kinematic.
if (activeA == false && activeB == false)
{
c = c.GetNext();
continue;
}
int proxyIdA = fixtureA.m_proxies[indexA].proxyId;
int proxyIdB = fixtureB.m_proxies[indexB].proxyId;
bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
b2Contact cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// The contact persists.
c.Update(m_contactListener);
c = c.GetNext();
}
}
// REVISADO =====================================================================
b2Fixture j2b2Fixture(b2Body body, JObject fixtureValue)
{
if (null == fixtureValue)
return null;
b2FixtureDef fixtureDef = new b2FixtureDef();
//Fixture fixtureDef = new Fixture();
fixtureDef.restitution = jsonToFloat("restitution", fixtureValue);
fixtureDef.friction = jsonToFloat("friction", fixtureValue);
fixtureDef.density = jsonToFloat("density", fixtureValue);
fixtureDef.isSensor = fixtureValue["sensor"] == null ? false : (bool)fixtureValue["sensor"];
fixtureDef.filter.categoryBits = (fixtureValue["filter-categoryBits"] == null) ? (ushort)0x0001 : (ushort)fixtureValue["filter-categoryBits"];
fixtureDef.filter.maskBits = fixtureValue["filter-maskBits"] == null ? (ushort)0xffff : (ushort)fixtureValue["filter-maskBits"];
fixtureDef.filter.groupIndex = fixtureValue["filter-groupIndex"] == null ? (short)0 : (short)fixtureValue["filter-groupIndex"];
b2Fixture fixture = null;
if (null != fixtureValue["circle"])
{
JObject circleValue = (JObject)fixtureValue["circle"];
b2CircleShape circleShape = new b2CircleShape();
circleShape.Radius = jsonToFloat("radius", circleValue);
circleShape.Position = jsonToVec("center", circleValue);
fixtureDef.shape = circleShape;
fixture = body.CreateFixture(fixtureDef);
}
else if (null != fixtureValue["edge"])
{
JObject edgeValue = (JObject)fixtureValue["edge"];
b2EdgeShape edgeShape = new b2EdgeShape();
edgeShape.Vertex1 = jsonToVec("vertex1", edgeValue);
edgeShape.Vertex2 = jsonToVec("vertex2", edgeValue);
edgeShape.HasVertex0 = edgeValue["hasVertex0"] == null ? false : (bool)edgeValue["hasVertex0"];
edgeShape.HasVertex3 = edgeValue["hasVertex3"] == null ? false : (bool)edgeValue["hasVertex3"];
if (edgeShape.HasVertex0)
edgeShape.Vertex0 = jsonToVec("vertex0", edgeValue);
if (edgeShape.HasVertex3)
edgeShape.Vertex3 = jsonToVec("vertex3", edgeValue);
fixtureDef.shape = edgeShape;
fixture = body.CreateFixture(fixtureDef);
}
else if (null != fixtureValue["loop"])
{// support old
// format (r197)
JObject chainValue = (JObject)fixtureValue["loop"];
b2ChainShape chainShape = new b2ChainShape();
int numVertices = ((JArray)chainValue["x"]).Count;
b2Vec2[] vertices = new b2Vec2[numVertices];
for (int i = 0; i < numVertices; i++)
vertices[i] = jsonToVec("vertices", chainValue, i);
chainShape.CreateLoop(vertices, numVertices);
fixtureDef.shape = chainShape;
fixture = body.CreateFixture(fixtureDef);
}
else if (null != fixtureValue["chain"])
{
// FPE. See http://www.box2d.org/forum/viewtopic.php?f=4&t=7973&p=35363
JObject chainValue = (JObject)fixtureValue["chain"];
b2ChainShape chainShape = new b2ChainShape();
int numVertices = ((JArray)chainValue["vertices"]["x"]).Count;
var vertices = new b2Vec2[numVertices];
for (int i = 0; i < numVertices; i++)
vertices[i] = jsonToVec("vertices", chainValue, i);
chainShape.CreateChain(vertices, numVertices);
chainShape.HasPrevVertex = chainValue["hasPrevVertex"] == null ? false : (bool)chainValue["hasPrevVertex"];
chainShape.HasNextVertex = chainValue["hasNextVertex"] == null ? false : (bool)chainValue["hasNextVertex"];
if (chainShape.HasPrevVertex)
chainShape.PrevVertex = (jsonToVec("prevVertex", chainValue));
if (chainShape.HasNextVertex)
chainShape.NextVertex = (jsonToVec("nextVertex", chainValue));
fixtureDef.shape = chainShape;
fixture = body.CreateFixture(fixtureDef);
}
else if (null != fixtureValue["polygon"])
{
JObject polygonValue = (JObject)fixtureValue["polygon"];
b2Vec2[] vertices = new b2Vec2[b2Settings.b2_maxPolygonVertices];
int numVertices = ((JArray)polygonValue["vertices"]["x"]).Count;
if (numVertices > b2Settings.b2_maxPolygonVertices)
{
Console.WriteLine("Ignoring polygon fixture with too many vertices.");
}
else if (numVertices < 2)
{
Console.WriteLine("Ignoring polygon fixture less than two vertices.");
}
else if (numVertices == 2)
{
Console.WriteLine("Creating edge shape instead of polygon with two vertices.");
b2EdgeShape edgeShape = new b2EdgeShape();
edgeShape.Vertex1 = jsonToVec("vertices", polygonValue, 0);
edgeShape.Vertex2 = jsonToVec("vertices", polygonValue, 1);
fixtureDef.shape = edgeShape;
fixture = body.CreateFixture(fixtureDef);
}
else
{
b2PolygonShape polygonShape = new b2PolygonShape();
for (int i = 0; i < numVertices; i++)
vertices[i] = jsonToVec("vertices", polygonValue, i);
polygonShape.Set(vertices, numVertices);
fixtureDef.shape = polygonShape;
fixture = body.CreateFixture(fixtureDef);
}
}
String fixtureName = fixtureValue["name"] == null ? "" : fixtureValue["name"].ToString();
if (fixtureName != "")
{
SetFixtureName(fixture, fixtureName);
}
return fixture;
}
public JObject B2n(b2Body body)
{
JObject bodyValue = new JObject();
string bodyName = GetBodyName(body);
if (null != bodyName)
bodyValue["name"] = bodyName;
switch (body.BodyType)
{
case b2BodyType.b2_staticBody:
bodyValue["type"] = 0;
break;
case b2BodyType.b2_kinematicBody:
bodyValue["type"] = 1;
break;
case b2BodyType.b2_dynamicBody:
bodyValue["type"] = 2;
break;
}
VecToJson("position", body.Position, bodyValue);
FloatToJson("angle", body.Angle, bodyValue);
VecToJson("linearVelocity", body.LinearVelocity, bodyValue);
FloatToJson("angularVelocity", body.AngularVelocity, bodyValue);
if (body.LinearDamping != 0)
FloatToJson("linearDamping", body.LinearDamping, bodyValue);
if (body.AngularDamping != 0)
FloatToJson("angularDamping", body.AngularDamping, bodyValue);
if (body.GravityScale != 1)
FloatToJson("gravityScale", body.GravityScale, bodyValue);
if (body.IsBullet())
bodyValue["bullet"] = true;
if (!body.IsSleepingAllowed())
bodyValue["allowSleep"] = false;
if (body.IsAwake())
bodyValue["awake"] = true;
if (!body.IsActive())
bodyValue["active"] = false;
if (body.IsFixedRotation())
bodyValue["fixedRotation"] = true;
b2MassData massData = new b2MassData();
massData = body.GetMassData();
if (massData.mass != 0)
FloatToJson("massData-mass", massData.mass, bodyValue);
if (massData.center.x != 0 || massData.center.y != 0)
VecToJson("massData-center", body.LocalCenter, bodyValue);
if (massData.I != 0)
{
FloatToJson("massData-I", massData.I, bodyValue);
}
//int i = 0;
JArray arr = new JArray();
b2Body tmp = body;
while (tmp != null)
{
bodyValue.Add("fixture", B2n(tmp));
tmp = body.Next;
}
bodyValue["fixture"] = arr;
JArray customPropertyValue = WriteCustomPropertiesToJson(body);
if (customPropertyValue.Count > 0)
bodyValue["customProperties"] = customPropertyValue;
return bodyValue;
}
public void SetCustomVector(b2Body item, String propertyName, b2Vec2 val)
{
m_bodiesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_vec2.Add(propertyName, val);
}
public void SetCustomString(b2Body item, String propertyName, String val)
{
m_bodiesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_string.Add(propertyName, val);
}
public void SetCustomFloat(b2Body item, String propertyName, float val)
{
m_bodiesWithCustomProperties.Add(item);
GetCustomPropertiesForItem(item, true).m_customPropertyMap_float.Add(propertyName, (float)val);
}
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);
}
// This is a callback from the broadphase when two AABB proxies begin
// to overlap. We create a b2Contact to manage the narrow phase.
public void AddPair(object proxyUserDataA, object proxyUserDataB)
{
b2Fixture fixtureA = proxyUserDataA as b2Fixture;
b2Fixture fixtureB = proxyUserDataB as b2Fixture;
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
b2ContactEdge edge = bodyB.GetContactList();
while (edge != null)
{
if (edge.other == bodyA)
{
b2Fixture fA = edge.contact.GetFixtureA();
b2Fixture fB = edge.contact.GetFixtureB();
if (fA == fixtureA && fB == fixtureB)
{
return;
}
if (fA == fixtureB && fB == fixtureA)
{
return;
}
}
edge = edge.next;
}
//Does a joint override collision? Is at least one body dynamic?
if (bodyB.ShouldCollide(bodyA) == false)
{
return;
}
// Check user filtering
if (m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Call the factory.
b2Contact c = m_contactFactory.Create(fixtureA, fixtureB);
// Contact creation may swap shapes.
fixtureA = c.GetFixtureA();
fixtureB = c.GetFixtureB();
bodyA = fixtureA.m_body;
bodyB = fixtureB.m_body;
// Insert into the world.
c.m_prev = null;
c.m_next = m_world.m_contactList;
if (m_world.m_contactList != null)
{
m_world.m_contactList.m_prev = c;
}
m_world.m_contactList = c;
// Connect to island graph.
// Connect to body A
c.m_nodeA.contact = c;
c.m_nodeA.other = bodyB;
c.m_nodeA.prev = null;
c.m_nodeA.next = bodyA.m_contactList;
if (bodyA.m_contactList != null)
{
bodyA.m_contactList.prev = c.m_nodeA;
}
bodyA.m_contactList = c.m_nodeA;
// Connect to body 2
c.m_nodeB.contact = c;
c.m_nodeB.other = bodyA;
c.m_nodeB.prev = null;
c.m_nodeB.next = bodyB.m_contactList;
if (bodyB.m_contactList != null)
{
bodyB.m_contactList.prev = c.m_nodeB;
}
bodyB.m_contactList = c.m_nodeB;
++m_world.m_contactCount;
return;
}
public void Destroy(b2Contact c)
{
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
if (m_contactListener && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
if (c.m_prev)
{
c.m_prev.m_next = c.m_next;
}
if (c.m_next)
{
c.m_next.m_prev = c.m_prev;
}
if (c == m_contactList)
{
m_contactList = c.m_next;
}
// Remove from body 1
if (c.m_nodeA.prev)
{
c.m_nodeA.prev.next = c.m_nodeA.next;
}
if (c.m_nodeA.next)
{
c.m_nodeA.next.prev = c.m_nodeA.prev;
}
if (c.m_nodeA == bodyA.m_contactList)
{
bodyA.m_contactList = c.m_nodeA.next;
}
// Remove from body 2
if (c.m_nodeB.prev)
{
c.m_nodeB.prev.next = c.m_nodeB.next;
}
if (c.m_nodeB.next)
{
c.m_nodeB.next.prev = c.m_nodeB.prev;
}
if (c.m_nodeB == bodyB.m_contactList)
{
bodyB.m_contactList = c.m_nodeB.next;
}
// Call the factory.
b2Contact.Destroy(c);
--m_contactCount;
}
public void clear()
{
m_mouseJoint = null;
m_mouseJointGroundBody = null;
}
public void AddBody(b2Body body)
{
//b2Settings.b2Assert(m_bodyCount < m_bodyCapacity);
body.m_islandIndex = m_bodyCount;
m_bodies[m_bodyCount++] = body;
}
public Car(
b2World b2world,
double width,
double length,
double[] position,
double angle,
double power,
double max_steer_angle,
double max_speed,
Wheel[] wheels
)
{
// /*
// pars is an object with possible attributes:
// width - width of the car in meters
// length - length of the car in meters
// position - starting position of the car, array [x, y] in meters
// angle - starting angle of the car, degrees
// max_steer_angle - maximum angle the wheels turn when steering, degrees
// max_speed - maximum speed of the car, km/h
// power - engine force, in newtons, that is applied to EACH powered wheel
// wheels - wheel definitions: [{x, y, rotatable, powered}}, ...] where
// x is wheel position in meters relative to car body center
// y is wheel position in meters relative to car body center
// revolving - boolean, does this turn rotate when steering?
// powered - is force applied to this wheel when accelerating/braking?
// */
// this.max_steer_angle=pars.max_steer_angle;
// this.max_speed=pars.max_speed;
// this.power=pars.power;
var wheel_angle = 0.0;//keep track of current wheel angle relative to car.
// //when steering left/right, angle will be decreased/increased gradually over 200ms to prevent jerkyness.
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = new b2Vec2(position[0], position[1]);
def.angle = angle.DegreesToRadians();
def.linearDamping = 0.15; //gradually reduces velocity, makes the car reduce speed slowly if neither accelerator nor brake is pressed
def.bullet = true; //dedicates more time to collision detection - car travelling at high speeds at low framerates otherwise might teleport through obstacles.
def.angularDamping = 0.3;
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1.0;
fixdef.friction = 0.3; //friction when rubbing agaisnt other shapes
fixdef.restitution = 0.4; //amount of force feedback when hitting something. >0 makes the car bounce off, it's fun!
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
body.CreateFixture(fixdef);
//initialize wheels
foreach (var item in wheels)
{
item.Initialize(this);
}
//return array of wheels that turn when steering
IEnumerable<Wheel> getRevolvingWheels = from w in wheels where w.revolving select w;
// //return array of powered wheels
IEnumerable<Wheel> getPoweredWheels = from w in wheels where w.powered select w;
#region setSpeed
Action<double> setSpeed = (speed) =>
{
/*
speed - speed in kilometers per hour
*/
var velocity0 = this.body.GetLinearVelocity();
//Console.WriteLine("car setSpeed velocity0 " + new { velocity0.x, velocity0.y });
var velocity2 = vectors.unit(new[] { velocity0.x, velocity0.y });
//Console.WriteLine("car setSpeed velocity2 " + new { x = velocity2[0], y = velocity2[1] });
var velocity = new b2Vec2(
velocity2[0] * ((speed * 1000.0) / 3600.0),
velocity2[1] * ((speed * 1000.0) / 3600.0)
);
//Console.WriteLine("car setSpeed SetLinearVelocity " + new { velocity.x, velocity.y });
this.body.SetLinearVelocity(velocity);
};
#endregion
#region getSpeedKMH
Func<double> getSpeedKMH = delegate
{
var velocity = this.body.GetLinearVelocity();
var len = vectors.len(new double[] { velocity.x, velocity.y });
return (len / 1000.0) * 3600.0;
};
#endregion
#region getLocalVelocity
Func<double[]> getLocalVelocity = delegate
{
/*
returns car's velocity vector relative to the car
*/
var retv = this.body.GetLocalVector(this.body.GetLinearVelocityFromLocalPoint(new b2Vec2(0, 0)));
return new double[] { retv.x, retv.y };
};
#endregion
#region update
this.update = (msDuration) =>
{
#region 1. KILL SIDEWAYS VELOCITY
//kill sideways velocity for all wheels
for (var i = 0; i < wheels.Length; i++)
{
wheels[i].killSidewaysVelocity();
}
#endregion
#region 2. SET WHEEL ANGLE
//calculate the change in wheel's angle for this update, assuming the wheel will reach is maximum angle from zero in 200 ms
var incr = (max_steer_angle / 200.0) * msDuration;
if (steer == STEER_RIGHT)
{
wheel_angle = Math.Min(Math.Max(wheel_angle, 0) + incr, max_steer_angle); //increment angle without going over max steer
}
else if (steer == STEER_LEFT)
{
wheel_angle = Math.Max(Math.Min(wheel_angle, 0) - incr, -max_steer_angle); //decrement angle without going over max steer
}
else
{
wheel_angle = 0;
}
//update revolving wheels
getRevolvingWheels.WithEach(
w => w.setAngle(wheel_angle)
);
#endregion
#region 3. APPLY FORCE TO WHEELS
var base_vect = new double[2]; //vector pointing in the direction force will be applied to a wheel ; relative to the wheel.
//if accelerator is pressed down and speed limit has not been reached, go forwards
var lessthanlimit = (getSpeedKMH() < max_speed);
var flag1 = (accelerate == ACC_ACCELERATE) && lessthanlimit;
if (flag1)
{
base_vect = new double[] { 0, -1 };
}
else if (accelerate == ACC_BRAKE)
{
//braking, but still moving forwards - increased force
if (getLocalVelocity()[1] < 0)
{
base_vect = new double[] { 0, 1.3 };
}
//going in reverse - less force
else
{
base_vect = new double[] { 0, 0.7 };
}
}
else
{
base_vect[0] = 0;
base_vect[1] = 0;
}
//multiply by engine power, which gives us a force vector relative to the wheel
var fvect = new double[] {
power * base_vect[0],
power * base_vect[1]
};
//apply force to each wheel
getPoweredWheels.WithEachIndex(
(w, i) =>
{
var wp = w.body.GetWorldCenter();
var wf = w.body.GetWorldVector(new b2Vec2(fvect[0], fvect[1]));
//Console.WriteLine("getPoweredWheels ApplyForce #" + i);
w.body.ApplyForce(wf, wp);
}
);
//if going very slow, stop - to prevent endless sliding
var veryslow = (getSpeedKMH() < 4);
var flag2 = veryslow && (accelerate == ACC_NONE);
if (flag2)
{
//Console.WriteLine("setSpeed 0");
setSpeed(0);
}
#endregion
};
#endregion
}
public override void EndContact(Box2D.Dynamics.Contacts.b2Contact contact)
{
if (contact.FixtureA.UserData != null && contact.FixtureB.UserData != null) {
switch ((WorldFixtureData)contact.FixtureA.UserData) {
case WorldFixtureData.playergroundsensor:
if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.platform) {
playerGroundBody = null;
playerGround = WorldFixtureData.air;
} else if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.jumppad) {
playerGroundJumpPad = null;
playerGround = WorldFixtureData.air;
}
else {
playerGround = WorldFixtureData.air;
}
break;
case WorldFixtureData.playerleftsensor:
WallContact = Direction.None;
break;
case WorldFixtureData.playerrightsensor:
WallContact = Direction.None;
break;
default:
switch ((WorldFixtureData)contact.FixtureB.UserData) {
case WorldFixtureData.playergroundsensor:
if ((WorldFixtureData)contact.FixtureA.UserData == WorldFixtureData.platform) {
playerGroundBody = null;
playerGround = WorldFixtureData.air;
} else if ((WorldFixtureData)contact.FixtureA.UserData == WorldFixtureData.jumppad) {
playerGroundJumpPad = null;
playerGround = WorldFixtureData.air;
} else {
playerGround = WorldFixtureData.air;
}
break;
case WorldFixtureData.playerleftsensor:
WallContact = Direction.None;
break;
case WorldFixtureData.playerrightsensor:
WallContact = Direction.None;
break;
}
break;
}
if ((WorldFixtureData)contact.FixtureA.UserData == WorldFixtureData.playergroundsensor && (WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.platform ||
(WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.playergroundsensor && (WorldFixtureData)contact.FixtureA.UserData == WorldFixtureData.platform) {
} else if ((WorldFixtureData)contact.FixtureA.UserData == WorldFixtureData.playergroundsensor || (WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.playergroundsensor) {
}
}
}
public void Add(b2Body body)
{
Debug.Assert(m_bodyCount < m_bodyCapacity);
body.IslandIndex = m_bodyCount;
m_bodies[m_bodyCount] = body;
++m_bodyCount;
}
public override void BeginContact(Box2D.Dynamics.Contacts.b2Contact contact)
{
base.BeginContact (contact);
if (contact.FixtureA.UserData != null && contact.FixtureB.UserData != null) {
switch ((WorldFixtureData)contact.FixtureA.UserData) {
case WorldFixtureData.playergroundsensor:
switch ((WorldFixtureData)contact.FixtureB.UserData) {
case WorldFixtureData.ground:
playerGround = WorldFixtureData.ground;
break;
case WorldFixtureData.platform:
playerGround = WorldFixtureData.platform;
playerGroundBody = contact.FixtureB.Body;
break;
case WorldFixtureData.jumppad:
playerGround = WorldFixtureData.jumppad;
playerGroundJumpPad = gameContainer.jumpPadContainer.First (search => search.JumpPadBody.Position == contact.FixtureB.Body.Position);
break;
default:
playerGround = WorldFixtureData.air;
break;
}
break;
case WorldFixtureData.playerleftsensor:
if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.ground) {
WallContact = Direction.Left;
} else {
WallContact = Direction.None;
}
break;
case WorldFixtureData.playerrightsensor:
if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.ground) {
WallContact = Direction.Right;
} else {
WallContact = Direction.None;
}
break;
default:
switch ((WorldFixtureData)contact.FixtureB.UserData) {
case WorldFixtureData.playergroundsensor:
switch ((WorldFixtureData)contact.FixtureA.UserData) {
case WorldFixtureData.ground:
playerGround = WorldFixtureData.ground;
break;
case WorldFixtureData.platform:
playerGround = WorldFixtureData.platform;
playerGroundBody = contact.FixtureA.Body;
break;
case WorldFixtureData.jumppad:
playerGround = WorldFixtureData.jumppad;
playerGroundJumpPad = gameContainer.jumpPadContainer.First (search => search.JumpPadBody.Position == contact.FixtureB.Body.Position);
break;
default:
playerGround = WorldFixtureData.air;
break;
}
break;
case WorldFixtureData.playerleftsensor:
if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.ground) {
WallContact = Direction.Left;
} else {
WallContact = Direction.None;
}
break;
case WorldFixtureData.playerrightsensor:
if ((WorldFixtureData)contact.FixtureB.UserData == WorldFixtureData.ground) {
WallContact = Direction.Right;
} else {
WallContact = Direction.None;
}
break;
}
break;
}
}
}
public void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep)
#endif
{
#if PROFILING
b2Timer timer = new b2Timer();
#endif
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b2Vec2 c = b.Sweep.c;
float a = b.Sweep.a;
b2Vec2 v = b.LinearVelocity;
float w = b.AngularVelocity;
// Store positions for continuous collision.
b.Sweep.c0 = b.Sweep.c;
b.Sweep.a0 = b.Sweep.a;
if (b.BodyType == b2BodyType.b2_dynamicBody)
{
// Integrate velocities.
v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force);
w += h * b.InvertedI * b.Torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
v *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
}
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
#if PROFILING
timer.Reset();
#endif
// Solver data
b2SolverData solverData = new b2SolverData();
solverData.step = step;
solverData.positions = m_positions;
solverData.velocities = m_velocities;
// Initialize velocity constraints.
b2ContactSolverDef contactSolverDef;
contactSolverDef.step = step;
contactSolverDef.contacts = m_contacts;
contactSolverDef.count = m_contactCount;
contactSolverDef.positions = m_positions;
contactSolverDef.velocities = m_velocities;
b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef);
contactSolver.InitializeVelocityConstraints();
if (step.warmStarting)
{
contactSolver.WarmStart();
}
for (int i = 0; i < m_jointCount; ++i)
{
m_joints[i].InitVelocityConstraints(solverData);
}
#if PROFILING
profile.solveInit = timer.GetMilliseconds();
#endif
// Solve velocity constraints
#if PROFILING
timer.Reset();
#endif
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < m_jointCount; ++j)
{
m_joints[j].SolveVelocityConstraints(solverData);
}
contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting
contactSolver.StoreImpulses();
#if PROFILING
profile.solveVelocity = timer.GetMilliseconds();
#endif
// Integrate positions
for (int i = 0; i < m_bodyCount; ++i)
{
b2Vec2 c = m_positions[i].c;
float a = m_positions[i].a;
b2Vec2 v = m_velocities[i].v;
float w = m_velocities[i].w;
// Check for large velocities
b2Vec2 translation = h * v;
if (translation.LengthSquared /* b2Math.b2Dot(translation, translation)*/ > b2Settings.b2_maxTranslationSquared)
{
float ratio = b2Settings.b2_maxTranslation / translation.Length;
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
m_positions[i].c = c;
m_positions[i].a = a;
m_velocities[i].v = v;
m_velocities[i].w = w;
}
// Solve position constraints
#if PROFILING
timer.Reset();
#endif
bool positionSolved = false;
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int i2 = 0; i2 < m_jointCount; ++i2)
{
bool jointOkay = m_joints[i2].SolvePositionConstraints(solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body body = m_bodies[i];
body.Sweep.c = m_positions[i].c;
body.Sweep.a = m_positions[i].a;
body.LinearVelocity = m_velocities[i].v;
body.AngularVelocity = m_velocities[i].w;
body.SynchronizeTransform();
}
#if PROFILING
profile.solvePosition = timer.GetMilliseconds();
#endif
Report(contactSolver.m_velocityConstraints);
if (allowSleep)
{
float minSleepTime = b2Settings.b2_maxFloat;
float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance;
float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance;
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.BodyType == b2BodyType.b2_staticBody)
{
continue;
}
if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) ||
b.AngularVelocity * b.AngularVelocity > angTolSqr ||
b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved)
{
for (int i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
b.SetAwake(false);
}
}
}
}
public b2Body(b2BodyDef bd, b2World world)
{
m_flags = 0;
if (bd.bullet)
{
m_flags |= b2BodyFlags.e_bulletFlag;
}
if (bd.fixedRotation)
{
m_flags |= b2BodyFlags.e_fixedRotationFlag;
}
if (bd.allowSleep)
{
m_flags |= b2BodyFlags.e_autoSleepFlag;
}
if (bd.awake)
{
m_flags |= b2BodyFlags.e_awakeFlag;
}
if (bd.active)
{
m_flags |= b2BodyFlags.e_activeFlag;
}
m_world = world;
m_xf.p = bd.position;
m_xf.q.Set(bd.angle);
m_sweep.localCenter.SetZero();
m_sweep.c0 = m_xf.p;
m_sweep.c = m_xf.p;
m_sweep.a0 = bd.angle;
m_sweep.a = bd.angle;
m_sweep.alpha0 = 0.0f;
m_jointList = null;
m_contactList = null;
m_prev = null;
m_next = null;
m_linearVelocity = bd.linearVelocity;
m_angularVelocity = bd.angularVelocity;
m_linearDamping = bd.linearDamping;
m_angularDamping = bd.angularDamping;
m_gravityScale = bd.gravityScale;
m_force.SetZero();
m_torque = 0.0f;
m_sleepTime = 0.0f;
m_type = bd.type;
if (m_type == b2BodyType.b2_dynamicBody)
{
m_mass = 1.0f;
m_invMass = 1.0f;
}
else
{
m_mass = 0.0f;
m_invMass = 0.0f;
}
m_I = 0.0f;
m_invI = 0.0f;
m_userData = bd.userData;
m_fixtureList = null;
m_fixtureCount = 0;
}
public virtual bool ShouldCollide(b2Body other)
{
// At least one body should be dynamic.
if (m_type != b2_dynamicBody && other.m_type != b2_dynamicBody)
{
return false;
}
// Does a joint prevent collision?
for (b2JointEdge* jn = m_jointList; jn; jn = jn.next)
{
if (jn.other == other)
{
if (jn.joint.m_collideConnected == false)
{
return false;
}
}
}
return true;
}
public Test()
{
m_destructionListener = new DestructionListener();
m_debugDraw = new CCBox2dDraw("fonts/arial-12", 1);
b2Vec2 gravity = new b2Vec2();
gravity.Set(0.0f, -10.0f);
m_world = new b2World(gravity);
m_bomb = null;
m_textLine = 30;
m_mouseJoint = null;
m_pointCount = 0;
m_destructionListener.test = this;
m_world.SetDestructionListener(m_destructionListener);
m_world.SetContactListener(this);
m_world.SetDebugDraw(m_debugDraw);
m_world.SetContinuousPhysics(true);
m_world.SetWarmStarting(true);
m_bombSpawning = false;
m_stepCount = 0;
b2BodyDef bodyDef = new b2BodyDef();
m_groundBody = m_world.CreateBody(bodyDef);
}
public void Destroy(b2Contact c)
{
b2Fixture fixtureA = c.FixtureA;
b2Fixture fixtureB = c.FixtureB;
b2Body bodyA = fixtureA.Body;
b2Body bodyB = fixtureB.Body;
if (m_contactListener != null && c.IsTouching())
{
m_contactListener.EndContact(c);
}
// Remove from the world.
if (c.Prev != null)
{
c.Prev.Next = c.Next;
}
if (c.Next != null)
{
c.Next.Prev = c.Prev;
}
if (c == m_contactList)
{
m_contactList = c.Next;
}
// Remove from body 1
if (c.NodeA.Prev != null)
{
c.NodeA.Prev.Next = c.NodeA.Next;
}
if (c.NodeA.Next != null)
{
c.NodeA.Next.Prev = c.NodeA.Prev;
}
if (c.NodeA == bodyA.ContactList)
{
bodyA.ContactList = c.NodeA.Next;
}
// Remove from body 2
if (c.NodeB.Prev != null)
{
c.NodeB.Prev.Next = c.NodeB.Next;
}
if (c.NodeB.Next != null)
{
c.NodeB.Next.Prev = c.NodeB.Prev;
}
if (c.NodeB == bodyB.ContactList)
{
bodyB.ContactList = c.NodeB.Next;
}
c.Free();
// Call the factory.
--m_contactCount;
}
public void LaunchBomb(b2Vec2 position, b2Vec2 velocity)
{
if (m_bomb != null)
{
m_world.DestroyBody(m_bomb);
m_bomb = null;
}
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position = position;
bd.bullet = true;
m_bomb = m_world.CreateBody(bd);
m_bomb.LinearVelocity = velocity;
b2CircleShape circle = new b2CircleShape();
circle.Radius = 0.3f;
b2FixtureDef fd = new b2FixtureDef();
fd.shape = circle;
fd.density = 20.0f;
fd.restitution = 0.0f;
b2Vec2 minV = position - new b2Vec2(0.3f, 0.3f);
b2Vec2 maxV = position + new b2Vec2(0.3f, 0.3f);
b2AABB aabb = new b2AABB();
aabb.LowerBound = minV;
aabb.UpperBound = maxV;
m_bomb.CreateFixture(fd);
}
public void DestroyBody(b2Body b)
{
if (IsLocked())
{
return;
}
// Delete the attached joints.
b2JointEdge je = b.JointList;
while (je)
{
b2JointEdge je0 = je;
je = je.next;
if (m_destructionListener != null)
{
m_destructionListener.SayGoodbye(je0.joint);
}
DestroyJoint(je0.joint);
b.JointList = je;
}
b.JointList = null;
// Delete the attached contacts.
b2ContactEdge ce = b.ContactList;
while (ce)
{
b2ContactEdge ce0 = ce;
ce = ce.next;
m_contactManager.Destroy(ce0.contact);
}
b.ContactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
b2Fixture f = b.FixtureList;
while (f != null)
{
b2Fixture f0 = f;
f = f.Next;
if (m_destructionListener != null)
{
m_destructionListener.SayGoodbye(f0);
}
f0.DestroyProxies(m_contactManager.BroadPhase);
b.FixtureList = f;
b.FixtureCount -= 1;
}
b.FixtureList = null;
b.FixtureCount = 0;
// Remove world body list.
if (b.Prev != null)
{
b.Prev.Next = b.Next;
}
if (b.Next != null)
{
b.Next.Prev = b.Prev;
}
if (b == m_bodyList)
{
m_bodyList = b.Next;
}
--m_bodyCount;
}
public virtual bool ShouldCollide(b2Body other)
{
// At least one body should be dynamic.
if (m_type != b2BodyType.b2_dynamicBody && other.m_type != b2BodyType.b2_dynamicBody)
{
return false;
}
// Does a joint prevent collision?
for (b2JointEdge jn = m_jointList; jn != null; jn = jn.Next)
{
if (jn.Other == other)
{
if (jn.Joint.GetCollideConnected() == false)
{
return false;
}
}
}
return true;
}
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 void SolveTOI(b2TimeStep subStep)
{
int i;
int j;
m_contactSolver.Initialize(subStep, m_contacts, m_contactCount, m_allocator);
b2ContactSolver contactSolver = m_contactSolver;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
// Warm starting for joints is off for now, but we need to
// call this function to compute Jacobians.
for (i = 0; i < m_jointCount; ++i)
{
m_joints[i].InitVelocityConstraints(subStep);
}
// Solve velocity constraints.
for (i = 0; i < subStep.velocityIterations; ++i)
{
contactSolver.SolveVelocityConstraints();
for (j = 0; j < m_jointCount; ++j)
{
m_joints[j].SolveVelocityConstraints(subStep);
}
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (i = 0; i < m_bodyCount; ++i)
{
b2Body b = m_bodies[i];
if (b.GetType() == b2Body.b2_staticBody)
{
continue;
}
// Check for large velocities.
// b2Vec2 translation = subStep.dt * b.m_linearVelocity;
float translationX = subStep.dt * b.m_linearVelocity.x;
float translationY = subStep.dt * b.m_linearVelocity.y;
//if (b2Dot(translation, translation) > b2_maxTranslationSquared)
if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared)
{
b.m_linearVelocity.Normalize();
b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * subStep.inv_dt;
b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * subStep.inv_dt;
}
float rotation = subStep.dt * b.m_angularVelocity;
if (rotation * rotation > b2Settings.b2_maxRotationSquared)
{
if (b.m_angularVelocity < 0.0f)
{
b.m_angularVelocity = -b2Settings.b2_maxRotation * subStep.inv_dt;
}
else
{
b.m_angularVelocity = b2Settings.b2_maxRotation * subStep.inv_dt;
}
}
// Store positions for continuous collision.
b.m_sweep.c0.SetV(b.m_sweep.c);
b.m_sweep.a0 = b.m_sweep.a;
// Integrate
b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x;
b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y;
b.m_sweep.a += subStep.dt * b.m_angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Solve position constraints.
float k_toiBaumgarte = 0.75f;
for (i = 0; i < subStep.positionIterations; ++i)
{
bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte);
bool jointsOkay = true;
for (j = 0; j < m_jointCount; ++j)
{
bool jointOkay = m_joints[j].SolvePositionConstraints(b2Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
break;
}
}
Report(contactSolver.m_constraints);
}
// 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 void Create(b2Body body, b2FixtureDef def)
{
m_userData = def.userData;
m_friction = def.friction;
m_restitution = def.restitution;
m_body = body;
Next = null;
m_filter = def.filter;
m_isSensor = def.isSensor;
m_shape = def.shape.Clone();
// Reserve proxy space
int childCount = m_shape.GetChildCount();
for (int i = 0; i < childCount; ++i)
{
b2FixtureProxy proxy = new b2FixtureProxy();
proxy.fixture = null;
proxy.proxyId = b2BroadPhase.e_nullProxy;
m_proxies.Add(proxy);
}
m_proxyCount = 0;
m_density = def.density;
}
// This is the top level collision call for the time step. Here
// all the narrow phase collision is processed for the world
// contact list.
public void Collide()
{
// Update awake contacts.
b2Contact c = m_world.m_contactList;
while (c != null)
{
b2Fixture fixtureA = c.GetFixtureA();
b2Fixture fixtureB = c.GetFixtureB();
b2Body bodyA = fixtureA.GetBody();
b2Body bodyB = fixtureB.GetBody();
if (bodyA.IsAwake() == false && bodyB.IsAwake() == false)
{
c = c.GetNext();
continue;
}
b2Contact cNuke;
// Is this contact flagged for filtering?
if ((c.m_flags & b2Contact.e_filterFlag) > 0)
{
// Should these bodies collide?
if (bodyB.ShouldCollide(bodyA) == false)
{
cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Check user filtering.
if (m_contactFilter.ShouldCollide(fixtureA, fixtureB) == false)
{
cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
// Clear the filtering flag
c.m_flags &= ~b2Contact.e_filterFlag;
}
object proxyA = fixtureA.m_proxy;
object proxyB = fixtureB.m_proxy;
bool overlap = m_broadPhase.TestOverlap(proxyA, proxyB);
// Here we destroy contacts that cease to overlap in the broadphase
if (overlap == false)
{
cNuke = c;
c = cNuke.GetNext();
Destroy(cNuke);
continue;
}
c.Update(m_contactListener);
c = c.GetNext();
}
}
public Wheel(
b2World b2world,
double x,
double y,
double width,
double length,
bool revolving,
bool powered
)
{
this.revolving = revolving;
this.powered = powered;
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);
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;
}
b2world.CreateJoint(jointdef);
#region setAngle
this.setAngle =
(angle) =>
{
/*
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 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 string GetBodyName(b2Body body)
{
if (m_bodyToNameMap.ContainsKey(body))
return m_bodyToNameMap[body];
return null;
}