public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, IContactListener listener)
{
_bodyCapacity = bodyCapacity;
_contactCapacity = contactCapacity;
_jointCapacity = jointCapacity;
_bodyCount = 0;
_jointCount = 0;
_listener = listener;
if (_bodies == null || _bodies.Length < bodyCapacity)
{
_bodies = new Body[bodyCapacity];
}
if (_contacts == null || _contacts.Length < contactCapacity)
{
_contacts = new Contact[contactCapacity * 2];
}
if (_joints == null || _joints.Length < jointCapacity)
{
_joints = new Joint[jointCapacity * 2];
}
}
public Player(GameContent gameContent, World world, Vector2 position)
{
this.gameContent = gameContent;
this.world = world;
idle = new Animation(gameContent.playerIdle, 2, 2f, true, new Vector2(0.5f));
walk = new Animation(gameContent.playerWalk, 2, 0.2f, true, new Vector2(0.5f));
die = new Animation(gameContent.playerDie, 2, 0.2f, false, new Vector2(0.5f));
animationPlayer.PlayAnimation(idle);
BodyDef bd = new BodyDef();
bd.position = position / gameContent.b2Scale;
bd.type = BodyType.Dynamic;
bd.linearDamping = 10;
body = world.CreateBody(bd);
CircleShape cs = new CircleShape();
cs._radius = (float)(idle.FrameWidth - 1) / gameContent.b2Scale / 2;
FixtureDef fd = new FixtureDef();
fd.shape = cs;
fd.filter.groupIndex = -1;
body.CreateFixture(fd);
}
public Enemy(World world, GameContent gameContent, int index, Vector2 position)
{
this.gameContent = gameContent;
walk = new Animation(gameContent.enemy[index], 2, 0.15f, true, new Vector2(0.5f));
animationPlayer.PlayAnimation(walk);
if (index == 0) linearImpulse = 1f / 2;
else linearImpulse = 0.75f / 2;
BodyDef bd = new BodyDef();
bd.position = position / gameContent.b2Scale;
bd.type = BodyType.Dynamic;
bd.linearDamping = 10;
body = world.CreateBody(bd);
CircleShape cs = new CircleShape();
cs._radius = (float)(walk.FrameWidth - 1) / gameContent.b2Scale / 2;
FixtureDef fd = new FixtureDef();
fd.shape = cs;
fd.filter.groupIndex = -1;
body.CreateFixture(fd);
body.SetUserData(this);
}
public void addPhysicsObj(GameObject _gameObj,Body _body)
{
PhysicsObj obj = new PhysicsObj(_gameObj, _body);
_gameObj.physicsObj = obj;
this.privActiveAddToFront((ManLink)obj, ref this.active);
}
public Soldier(Shape shape, Vector2 position, GameContent gameContent, World world)
{
this.gameContent = gameContent;
this.world = world;
idle = new Animation(gameContent.idle[(int)shape], 20f, false);
walk = new Animation(gameContent.walk[(int)shape], 0.15f, true);
animationPlayer.PlayAnimation(idle);
MaxHealth = 10; health = gameContent.random.Next(2, 10);
MaxReloadTime = gameContent.random.Next(50); reloadTime = 0;
CircleShape cShape = new CircleShape();
cShape._radius = (Size + 2) / 2 / gameContent.b2Scale;
BodyDef bd = new BodyDef();
bd.fixedRotation = true;
bd.type = BodyType.Dynamic;
bd.position = position / gameContent.b2Scale;
body = world.CreateBody(bd);
//body.SetLinearDamping(10);
FixtureDef fd = new FixtureDef();
fd.shape = cShape;
fd.restitution = 0.5f;
fd.friction = .1f;
fd.density = .1f;
body.CreateFixture(fd);
body.SetUserData(this);
}
public Car(Vector2 position, GameContent gameContent, World world)
{
this.world = world;
this.gameContent = gameContent;
BodyDef bd = new BodyDef();
bd.position = position / gameContent.Scale;
bd.type = BodyType.Dynamic;
bd.bullet = true;
body = world.CreateBody(bd);
body.SetLinearDamping(1f); body.SetAngularDamping(0.1f);
float width = gameContent.playerCar.Width, height = gameContent.playerCar.Height;
FixtureDef fd = new FixtureDef();
fd.density = 0.1f;
//fd.restitution = .1f;
CircleShape cs = new CircleShape();
cs._p = new Vector2(0, -(height - width / 2)) / gameContent.Scale;
cs._radius = width / 2 / gameContent.Scale;
fd.shape = cs; body.CreateFixture(fd);
PolygonShape ps = new PolygonShape();
ps.SetAsBox(width / 2 / gameContent.Scale, (height - width / 2) / 2 / gameContent.Scale,
new Vector2(0, -(height - width / 2) / 2) / gameContent.Scale, 0);
fd.shape = ps; body.CreateFixture(fd);
CreateWheels();
}
public override bool init()
{
if (!base.init())
return false;
if (!base.init())
{
return false;
}
CCSize winSize = CCDirector.sharedDirector().getWinSize();
title = CCLabelTTF.labelWithString("FootBall", "Arial", 24);
title.position = new CCPoint(winSize.width / 2, winSize.height - 50);
this.addChild(title, 1);
ball = CCSprite.spriteWithFile(@"images/ball");
ball.position = new CCPoint(100, 300);
this.addChild(ball);
Vector2 gravity = new Vector2(0.0f, -30.0f);
bool doSleep = true;
world = new World(gravity, doSleep);
/////////////////////////
BodyDef groundBodyDef = new BodyDef();
groundBodyDef.position = new Vector2(0, 0);
Body groundBody = world.CreateBody(groundBodyDef);
PolygonShape groundBox = new PolygonShape();
FixtureDef boxShapeDef = new FixtureDef();
boxShapeDef.shape = groundBox;
groundBox.SetAsEdge(new Vector2(0, 0), new Vector2((float)(winSize.width / PTM_RATIO), 0));
groundBody.CreateFixture(boxShapeDef);
groundBox.SetAsEdge(new Vector2(0, 0), new Vector2(0, (float)(winSize.height / PTM_RATIO)));
groundBody.CreateFixture(boxShapeDef);
groundBox.SetAsEdge(new Vector2(0, (float)(winSize.height / PTM_RATIO)),
new Vector2((float)(winSize.width / PTM_RATIO), (float)(winSize.height / PTM_RATIO)));
groundBody.CreateFixture(boxShapeDef);
groundBox.SetAsEdge(new Vector2((float)(winSize.width / PTM_RATIO), (float)(winSize.height / PTM_RATIO)),
new Vector2((float)(winSize.width / PTM_RATIO), 0));
groundBody.CreateFixture(boxShapeDef);
BodyDef ballBodyDef = new BodyDef();
ballBodyDef.type = BodyType.Dynamic;
ballBodyDef.position = new Vector2(
(float)(100 / PTM_RATIO),
(float)(300 / PTM_RATIO));
ballBodyDef.userData = ball;
body = world.CreateBody(ballBodyDef);
CircleShape circle = new CircleShape();
circle._radius = (float)(26.0 / PTM_RATIO);
FixtureDef ballShapeDef = new FixtureDef();
ballShapeDef.shape = circle;
ballShapeDef.density = 1.0f;
ballShapeDef.friction = 0.0f;
ballShapeDef.restitution = 1.0f;
body.CreateFixture(ballShapeDef);
this.schedule(tick);
return true;
}
public Terrain(World world)
{
mWorld = world;
mBodyDef = new BodyDef();
mBody = world.CreateBody(mBodyDef);
Generate();
GenerateDrawVertexs();
}
// Point-to-point constraint
// C = p2 - p1
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// C = angle2 - angle1 - referenceAngle
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body b1, Body b2, Vector2 anchor)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
referenceAngle = bodyB.GetAngle() - bodyA.GetAngle();
}
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
public void Initialize(Body b1, Body b2, Vector2 anchor, Vector2 axis)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor);
localAnchorB = bodyB.GetLocalPoint(anchor);
localAxisA = bodyA.GetLocalVector(axis);
}
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
public void Initialize(Body b1, Body b2,
Vector2 anchor1, Vector2 anchor2)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vector2 anchor1, Vector2 anchor2, float maxlength)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
length = maxlength;
}
public DrawUnit(Sprite sprite, Body body, World world)
: base(body, world)
{
_sprite = sprite;
_eventManager = world.EventManager;
_listener = new EventListener(Constants.TIME_TICKS_EVENT);
_listener.Handler = e => Update();
_eventManager.Register(_listener);
}
public Apple(Vector2 position, Body ground, GameContent gameContent, World world)
: this(null, gameContent, world)
{
this.ground = ground;
body.Position = position;
originalPostion = position;
State = LeafState.Drop;
fallenFromAir = true;
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
// 1-D rained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
public void Initialize(Body b1, Body b2,
Vector2 anchor1, Vector2 anchor2)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
Vector2 d = anchor2 - anchor1;
length = d.Length();
}
public void PostSolve(Contact contact, ref ContactImpulse impulse)
{
Body[] bodies = new Body[2];
bodies[0] = contact.GetFixtureA().GetBody();
bodies[1] = contact.GetFixtureB().GetBody();
mBodies.Add(bodies);
if (mBodies.Count > 10000)
{
mBodies.Clear();
Console.WriteLine("Clear contacts.");
}
}
public Asteroid(GameContent gameContent, int index, Body body)
{
this.gameContent = gameContent;
tex = gameContent.asteroid[index];
this.body = body;
body.SetUserData(this);
originalPosition = body.Position;
for (int i = 0; i < 5; i++)
blastTex[i] = gameContent.asteroid[(index + i) % 16];
blastSound = gameContent.boom[index % 3];
blastSoundInstance = blastSound.CreateInstance();
blastSoundInstance.Volume = 0.8f;
}
public void addBodyToDestroy(Body b)
{
BodyNode bodyNode = new BodyNode(b);
if (destroyHead == null)
{
destroyHead = bodyNode;
bodyNode.next = null;
bodyNode.prev = null;
}
else
{
bodyNode.next = destroyHead;
destroyHead.prev = bodyNode;
destroyHead = bodyNode;
}
}
public Box(World world, Vector2 position, Vector2 size, string texture, bool isStatic, Player player, float health = 100)
{
ObjectType = EObjectType.Box;
mHealth = health;
mStartHealth = health;
mIsDestroyed = false;
mSize = size;
mWorld = world;
mTexture = texture;
mPlayer = player;
DestroyTime = null;
PolygonShape polygonShape = new PolygonShape();
polygonShape.SetAsBox(size.X / 2f, size.Y / 2f);
BodyDef bodyDef = new BodyDef();
bodyDef.position = position;
bodyDef.bullet = true;
if (isStatic)
{
bodyDef.type = BodyType.Static;
}
else
{
bodyDef.type = BodyType.Dynamic;
}
mBody = world.CreateBody(bodyDef);
FixtureDef fixtureDef = new FixtureDef();
fixtureDef.shape = polygonShape;//Форма
fixtureDef.density = 0.1f;//Плотность
fixtureDef.friction = 0.3f;//Сила трения
fixtureDef.restitution = 0f;//Отскок
Filter filter = new Filter();
filter.maskBits = (ushort)(EntityCategory.Player1 | EntityCategory.Player2);
filter.categoryBits = (ushort)player.PlayerType;
var fixture = mBody.CreateFixture(fixtureDef);
fixture.SetFilterData(ref filter);
MassData data = new MassData();
data.mass = 0.01f;
mBody.SetUserData(this);
}
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
public void Initialize(Body b1, Body b2,
Vector2 ga1, Vector2 ga2,
Vector2 anchor1, Vector2 anchor2,
float r)
{
bodyA = b1;
bodyB = b2;
groundAnchorA = ga1;
groundAnchorB = ga2;
localAnchorA = bodyA.GetLocalPoint(anchor1);
localAnchorB = bodyB.GetLocalPoint(anchor2);
Vector2 d1 = anchor1 - ga1;
lengthA = d1.Length();
Vector2 d2 = anchor2 - ga2;
lengthB = d2.Length();
ratio = r;
Debug.Assert(ratio > Settings.b2_epsilon);
float C = lengthA + ratio * lengthB;
maxLengthA = C - ratio * b2_minPulleyLength;
maxLengthB = (C - b2_minPulleyLength) / ratio;
}
public Branch(Path2D path, GameContent gameContent, World world, Branch nearestBranch)
{
this.gameContent = gameContent;
dot = gameContent.dot;
circle = gameContent.jointCircle;
square = gameContent.jointSquare;
cursor = gameContent.cursor;
BodyDef bd = new BodyDef();
bd.position = path.Keys[0];
bd.type = BodyType.Dynamic;
body = world.CreateBody(bd);
fixtureCount = path.Keys.Count;
for (int i = 0; i < path.Keys.Count; i++)
CreateFixture(path.Keys[i] - path.Keys[0]);
if (nearestBranch != null)
{
RevoluteJointDef revJd = new RevoluteJointDef();
revJd.bodyA = body;
revJd.bodyB = nearestBranch.body;
revJd.localAnchorA = Vector2.Zero;
AABB aabb; nearestBranch.nearestFixture.GetAABB(out aabb);
Vector2 p = aabb.GetCenter();
revJd.localAnchorB = nearestBranch.nearestFixture.GetBody().GetLocalPoint(p);
revJd.enableMotor = true;
revJd.referenceAngle = nearestBranch.nearestFixture.GetBody().Rotation;
revoJoint = (RevoluteJoint)world.CreateJoint(revJd);
revoJoint.SetUserData((Branch)nearestBranch);
nearestBranch.nearestFixture.SetUserData((Branch)this);
}
newGrow = true;
}
public Equipment(EquipmentName equipName, GameContent gameContent, World world)
{
this.gameContent = gameContent;
this.world = world;
this.equipName = equipName;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
body = world.CreateBody(bd);
equipmentData = gameContent.content.Load<EquipmentData>("Graphics/" + equipName.ToString());
topLeftVertex = equipmentData.TopLeftVertex;
origin = equipmentData.Origin;
rotationButtonPos = equipmentData.RotationButtonPosition;
rightClampPositionX = equipmentData.ClampData.RightClampPositionX;
clampRotation = equipmentData.ClampData.RotationInDeg / 180f * (float)Math.PI;
clampEnabled = equipmentData.ClampData.ClampEnabled;
SetFixtures();
// Collide only with Ground but not with itself and bonded Filter
mouseFilter = new Filter();
mouseFilter.categoryBits = 0x0002; mouseFilter.maskBits = 0x0001; mouseFilter.groupIndex = -2;
// Collide with every thing
groundFilter = new Filter();
groundFilter.categoryBits = 0x0001; groundFilter.maskBits = 65535; groundFilter.groupIndex = 0;
equipFilter = new Filter();
equipFilter.categoryBits = 0x0002; equipFilter.maskBits = 0x0001; equipFilter.groupIndex = 2;
//SetMode(false, false);
body.SetUserData((EquipmentName)equipName);
}
public Level(ScreenManager screenManager, int levelIndex)
{
this.gameContent = screenManager.GameContent;
this.levelIndex = levelIndex;
camera = new Camera2D();
groundBody = world.CreateBody(new BodyDef());
LoadTiles(levelIndex);
UpdateCameraChaseTarget(new GameTime());
Vector2[] v = {Vector2.Zero, new Vector2(tiles.GetLength(0), 0),
new Vector2(tiles.GetLength(0), tiles.GetLength(1)),
new Vector2(0, tiles.GetLength(1)), Vector2.Zero};
for (int i = 0; i < v.Length - 1; i++)
{
PolygonShape ps = new PolygonShape();
ps.SetAsEdge(v[i] * Tile.Width / gameContent.b2Scale, v[i + 1] * Tile.Width / gameContent.b2Scale);
groundBody.CreateFixture(ps, 0);
}
}
public Level(GameContent gameContent)
{
world = new World(atomGravity, false);
this.gameContent = gameContent;
if (gameContent.levelIndex == -1) this.isLAB = true;
camera = new Camera2D();
if (!isLAB) LoadLevelComponent();
else
{
labComponent = new LabComponent(gameContent, world);
editMode = true;
world.Gravity = equipGravity;
camera.Scale = targetScale = scale = labComponent.EqScale;
camera.Position = new Vector2(labComponent.Width, labComponent.Height) / 2 * Camera2D.PhoneScale;
camera.Speed = 500;
camera.ScrollBar = new Vector2(50, 50);
camera.ScrollArea = new Vector2(labComponent.Width, labComponent.Height) * Camera2D.PhoneScale;
}
mouseGroundBody = world.CreateBody(new BodyDef());
}
/// <summary> Destroy a joint. This may cause the connected bodies to begin colliding.
/// </summary> <warning> This function is locked during callbacks.
/// </warning>
public void DestroyJoint(Joint j)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
bool collideConnected = j._collideConnected;
// Remove from the doubly linked list.
if (j._prev != null)
{
j._prev._next = j._next;
}
if (j._next != null)
{
j._next._prev = j._prev;
}
if (j == _jointList)
{
_jointList = j._next;
}
// Disconnect from island graph.
Body bodyA = j._bodyA;
Body bodyB = j._bodyB;
// Wake up connected bodies.
bodyA.SetAwake(true);
bodyB.SetAwake(true);
// Remove from body 1.
if (j._edgeA.Prev != null)
{
j._edgeA.Prev.Next = j._edgeA.Next;
}
if (j._edgeA.Next != null)
{
j._edgeA.Next.Prev = j._edgeA.Prev;
}
if (j._edgeA == bodyA._jointList)
{
bodyA._jointList = j._edgeA.Next;
}
j._edgeA.Prev = null;
j._edgeA.Next = null;
// Remove from body 2
if (j._edgeB.Prev != null)
{
j._edgeB.Prev.Next = j._edgeB.Next;
}
if (j._edgeB.Next != null)
{
j._edgeB.Next.Prev = j._edgeB.Prev;
}
if (j._edgeB == bodyB._jointList)
{
bodyB._jointList = j._edgeB.Next;
}
j._edgeB.Prev = null;
j._edgeB.Next = null;
Debug.Assert(_jointCount > 0);
--_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge edge = bodyB.GetContactList();
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;
}
}
}
/// <summary>
/// Call this to initialize a Behaviour with data supplied in a file.
/// </summary>
/// <param name="fileName">The file to load from.</param>
public override void LoadContent(String fileName)
{
base.LoadContent(fileName);
// Load the definiton for this object.
SimulatedPhysicsDefinition def = GameObjectManager.pInstance.pContentManager.Load<SimulatedPhysicsDefinition>(fileName);
// Create the body representing this object in the physical world.
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = PhysicsManager.pInstance.ScreenToPhysicalWorld(new Vector2(640.0f, 340.0f));
bd.fixedRotation = true;
bd.linearDamping = 1.0f;
mBody = PhysicsManager.pInstance.pWorld.CreateBody(bd);
/*
// A bunch of code to create a capsule. Not using it because it has all the same problems as
// a box when it comes to catching edges and such.
var shape = new PolygonShape();
Vector2[] v = new Vector2[8];
Double increment = System.Math.PI * 2.0 / (v.Length - 2);
Double theta = 0.0;
Vector2 center = PhysicsManager.pInstance.ScreenToPhysicalWorld(new Vector2(0.0f, 8.0f));
Single radius = PhysicsManager.pInstance.ScreenToPhysicalWorld(8.0f);
for (Int32 i = 0; i < v.Length; i++)
{
v[i] = center + radius * new Vector2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
if (i == (v.Length / 2) - 1)
{
center = PhysicsManager.pInstance.ScreenToPhysicalWorld(new Vector2(0.0f, -8.0f));
i++;
v[i] = center + radius * new Vector2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
}
theta += increment;
}
//v = v.Reverse().ToArray();
shape.Set(v, v.Length);
var fd = new FixtureDef();
fd.shape = shape;
fd.restitution = 0.0f;
fd.friction = 0.5f;
fd.density = 1.0f;
mBody.CreateFixture(fd);
*/
var shape = new PolygonShape();
shape.SetAsBox(PhysicsManager.pInstance.ScreenToPhysicalWorld(8.0f), PhysicsManager.pInstance.ScreenToPhysicalWorld(8.0f));
var fd = new FixtureDef();
fd.shape = shape;
fd.restitution = 0.0f;
fd.friction = 0.5f;
fd.density = 1.0f;
//mBody.CreateFixture(fd);
var circle = new CircleShape();
circle._radius = PhysicsManager.pInstance.ScreenToPhysicalWorld(8);
circle._p = PhysicsManager.pInstance.ScreenToPhysicalWorld(new Vector2(0.0f, 8.0f));
fd = new FixtureDef();
fd.shape = circle;
fd.restitution = 0.0f;
fd.friction = 0.5f;
fd.density = 1.5f;
mBody.CreateFixture(fd);
circle = new CircleShape();
circle._radius = PhysicsManager.pInstance.ScreenToPhysicalWorld(8);
circle._p = PhysicsManager.pInstance.ScreenToPhysicalWorld(new Vector2(0.0f, -8.0f));
fd = new FixtureDef();
fd.shape = circle;
fd.restitution = 0.0f;
fd.friction = 0.5f;
fd.density = 1.5f;
mBody.CreateFixture(fd);
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
// Re-enable this contact.
_flags |= ContactFlags.Enabled;
bool touching = false;
bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;
bool sensorA = _fixtureA.IsSensor();
bool sensorB = _fixtureB.IsSensor();
bool sensor = sensorA || sensorB;
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Transform xfA; bodyA.GetTransform(out xfA);
Transform xfB; bodyB.GetTransform(out xfB);
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
_manifold._pointCount = 0;
}
else
{
Evaluate(ref _manifold, ref xfA, ref xfB);
touching = _manifold._pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold._pointCount; ++i)
{
ManifoldPoint mp2 = _manifold._points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
bool found = false;
for (int j = 0; j < oldManifold._pointCount; ++j)
{
ManifoldPoint mp1 = oldManifold._points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
found = true;
break;
}
}
if (found == false)
{
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
}
_manifold._points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
_flags |= ContactFlags.Touching;
}
else
{
_flags &= ~ContactFlags.Touching;
}
if (wasTouching == false && touching == true && null != listener)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && null != listener)
{
listener.EndContact(this);
}
if (sensor == false && null != listener)
{
listener.PreSolve(this, ref oldManifold);
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
}
/// <summary> Destroy a rigid body given a definition. No reference to the definition
/// is retained. This function is locked during callbacks.
/// </summary> <warning> This automatically deletes all associated shapes and joints.
/// </warning> <warning> This function is locked during callbacks.
/// </warning>
public void DestroyBody(Body b)
{
Debug.Assert(_bodyCount > 0);
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
// Delete the attached joints.
JointEdge je = b._jointList;
while (je != null)
{
JointEdge je0 = je;
je = je.Next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(je0.Joint);
}
DestroyJoint(je0.Joint);
}
b._jointList = null;
// Delete the attached contacts.
ContactEdge ce = b._contactList;
while (ce != null)
{
ContactEdge ce0 = ce;
ce = ce.Next;
_contactManager.Destroy(ce0.Contact);
}
b._contactList = null;
// Delete the attached fixtures. This destroys broad-phase proxies.
Fixture f = b._fixtureList;
while (f != null)
{
Fixture f0 = f;
f = f._next;
if (DestructionListener != null)
{
DestructionListener.SayGoodbye(f0);
}
f0.DestroyProxies(_contactManager._broadPhase);
f0.Destroy();
}
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 == _bodyList)
{
_bodyList = b._next;
}
--_bodyCount;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpper)
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpper)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
float C = _maxLength1 - length1;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpper)
{
Transform xf2;
b2.GetTransform(out xf2);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p2 = b2._sweep.c + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _maxLength2 - length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.b2_linearSlop);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.c + r2 - b1._sweep.c - r1;
// Handle singularity.
float length = _u.Length();
if (length < _length)
{
return;
}
if (length > Settings.b2_linearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = new Vector2(0.0f, 0.0f);
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
Debug.Assert(invMass > Settings.b2_epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * _mass * _dampingRatio * omega;
// Spring stiffness
float k = _mass * omega * omega;
// magic formulas
_gamma = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
public void Add(Body body)
{
Debug.Assert(_bodyCount < _bodyCapacity);
body._islandIndex = _bodyCount;
_bodies[_bodyCount++] = body;
}
/// <summary>
/// Adds a polygon to a body
/// </summary>
/// <param name="body">The body where the polygon should be added</param>
/// <param name="vertices">The vertices of the polygon</param>
/// <param name="density">The density of the polygon</param>
/// <param name="friction">The friction of the polygon</param>
/// <param name="restitution">The restitution of the polygon</param>
private void AddPolygonToBody(Body body, Vector2[] vertices, float density, float friction,
float restitution)
{
PolygonShape shape = new PolygonShape();
shape.Set(vertices, vertices.Length);
FixtureDef fixtureDef = new FixtureDef();
fixtureDef.shape = shape;
fixtureDef.density = density;
fixtureDef.friction = friction;
fixtureDef.restitution = restitution;
body.CreateFixture(fixtureDef);
}
/// <summary>
/// Creates a new motorcycle and a driver into the given Box2D world.
/// Creates all the parts of the motorcycle and driver and joints them together.
/// </summary>
/// <param name="pBikeSpeed">A pointer to the variable that describes the speed of the
/// motorcycle</param>
/// <param name="pRotationData">RotationData to provide the information of the rotation
/// of the device</param>
/// <param name="pWorld">The Box2D world where the bike is created into</param>
/// <param name="pCamPos">A pointer to the variable that describes the position of the
/// camera</param>
/// <param name="pContent">The used ContentManager instance</param>
/// <param name="pSpriteBatch">The used SpriteBatch instance</param>
public Bike(float []pBikeSpeed, RotationData pRotationData, World pWorld, float[] pCamPos,
ContentManager pContent)
{
OffTheBike = false;
camPos = pCamPos;
world = pWorld;
content = pContent;
RotationData = pRotationData;
bikeSpeed = pBikeSpeed;
frontWheel = CreateCirclePart("wheel", frontWheelInitPos, 35.0f, 0, 0.1f, 0.9f, 0.2f);
frontFork = CreateBoxPart("susp_lower_long", frontForkInitPos, 20.53f, 21.33f, 0, 0.8f,
1.0f, 0.2f);
rearWheel = CreateCirclePart("rearWheel", rearWheelInitPos, 32.0f, 0, 0.4f, 1.0f,
0.2f);
rearFork = CreateBoxPart("rearFork", rearForkInitPos, 64.0f, 17.0f, rearForkInitRot,
0.5f, 1.0f, 0.2f);
bikeBody = CreateBikeBody(bikeBodyInitPos, bikeBodyInitRot, 0.5f, 1.0f, 0.2f);
RevoluteJointDef motorDef = new RevoluteJointDef();
motorDef.Initialize(rearWheel, rearFork, rearWheel.GetWorldCenter());
motorDef.maxMotorTorque = 2.0f;
motorDef.enableMotor = true;
motor = (RevoluteJoint)world.CreateJoint(motorDef);
RevoluteJointDef rearForkBodyDef = new RevoluteJointDef();
Vector2 anchor = rearFork.GetWorldCenter();
anchor.X += (32.0f / Level.FACTOR);
anchor.Y += (13.5f / Level.FACTOR);
rearForkBodyDef.Initialize(rearFork, bikeBody, anchor);
rearForkBodyDef.bodyA = rearFork;
rearForkBodyDef.bodyB = bikeBody;
rearForkBodyDef.maxMotorTorque = 300.0f;
world.CreateJoint(rearForkBodyDef);
RevoluteJointDef frontWheelJointDef = new RevoluteJointDef();
frontWheelJointDef.Initialize(frontWheel, frontFork, frontWheel.GetWorldCenter());
frontWheelJointDef.maxMotorTorque = 300.0f;
world.CreateJoint(frontWheelJointDef);
DistanceJointDef frontSuspToBikeDef = new DistanceJointDef();
frontSuspToBikeDef.Initialize(bikeBody, frontFork,
frontFork.GetWorldCenter() + new Vector2(0, 0.4f),
frontFork.GetWorldCenter());
frontSuspToBikeDef.frequencyHz = 4.0f;
frontSuspToBikeDef.dampingRatio = 0.1f;
frontSuspToBikeDef.collideConnected = true;
world.CreateJoint(frontSuspToBikeDef);
DistanceJointDef rearForkDistanceDef = new DistanceJointDef();
rearForkDistanceDef.Initialize(bikeBody, rearFork,
rearFork.GetWorldCenter() + new Vector2(0, 0.4f),
rearFork.GetWorldCenter());
rearForkDistanceDef.frequencyHz = 7.0f;
rearForkDistanceDef.dampingRatio = 0.1f;
rearForkDistanceDef.collideConnected = true;
world.CreateJoint(rearForkDistanceDef);
PrismaticJointDef fSuspBikePrismaticDef = new PrismaticJointDef();
fSuspBikePrismaticDef.Initialize(bikeBody, frontFork, bikeBody.GetWorldCenter(),
new Vector2(0, 1));
fSuspBikePrismaticDef.enableLimit = true;
fSuspBikePrismaticDef.lowerTranslation = -0.2f;
fSuspBikePrismaticDef.upperTranslation = 0.2f;
fSuspBikePrismaticDef.collideConnected = true;
world.CreateJoint(fSuspBikePrismaticDef);
humanBody = CreateBoxPart("human", humanBodyInitPos, 17.0f, 64.0f, 0, 0.1f, 1.0f,
0.2f);
head = CreateBoxPart("head", headInitPos, 38.4f, 29.9f, headInitRot, 0.1f, 1.0f,
0.2f);
hand = CreateBoxPart("hand", handInitPos, 34.13f, 8.53f, handInitRot, 0.1f, 1.0f,
0.2f);
arm = CreateBoxPart("arm", armInitPos, 42.67f, 8.53f, armInitRot, 0.1f, 1.0f, 0.2f);
WeldJointDef headToHumanDef = new WeldJointDef();
headToHumanDef.Initialize(head, humanBody, head.GetWorldCenter());
world.CreateJoint(headToHumanDef);
RevoluteJointDef humanToBikeDef = new RevoluteJointDef();
anchor = humanBody.GetWorldCenter();
anchor.Y += (30.0f / Level.FACTOR);
humanToBikeDef.Initialize(humanBody, bikeBody, anchor);
humanToBikeDef.maxMotorTorque = 300.0f;
humanToBike = world.CreateJoint(humanToBikeDef);
RevoluteJointDef humanToArmDef = new RevoluteJointDef();
anchor = arm.GetWorldPoint(new Vector2(-21.33f / Level.FACTOR, 4.26f / Level.FACTOR));
humanToArmDef.Initialize(humanBody, arm, anchor);
humanToArmDef.maxMotorTorque = 300.0f;
world.CreateJoint(humanToArmDef);
RevoluteJointDef armToHandDef = new RevoluteJointDef();
anchor = arm.GetWorldPoint(new Vector2(21.33f / Level.FACTOR, 4.26f / Level.FACTOR));
armToHandDef.Initialize(arm, hand, anchor);
armToHandDef.maxMotorTorque = 300.0f;
world.CreateJoint(armToHandDef);
RevoluteJointDef handToBikeDef = new RevoluteJointDef();
anchor = hand.GetWorldPoint(new Vector2(17.06f / Level.FACTOR, 4.26f / Level.FACTOR));
handToBikeDef.Initialize(hand, bikeBody, anchor);
handToBikeDef.maxMotorTorque = 300.0f;
handToBike = world.CreateJoint(handToBikeDef);
DistanceJointDef armToBikeDef = new DistanceJointDef();
armToBikeDef.Initialize(hand, bikeBody,
hand.GetWorldPoint(new Vector2(-17.00f / Level.FACTOR,
4.26f / Level.FACTOR)),
bikeBody.GetWorldCenter());
armToBikeDef.length = 40.0f / Level.FACTOR;
armToBikeDef.frequencyHz = 10.0f;
armToBikeDef.dampingRatio = 1.0f;
armToBikeDef.collideConnected = true;
armToBike = world.CreateJoint(armToBikeDef);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.b2_epsilon);
Debug.Assert(_limitMass2 > Settings.b2_epsilon);
Debug.Assert(_pulleyMass > Settings.b2_epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.warmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
/// <summary>
/// Call this to put a message back to its default state.
/// </summary>
public override void Reset()
{
mBody_Out = null;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.b2_epsilon);
_gamma = step.dt * (d + step.dt * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.col1.X += _gamma;
K.col2.Y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.c + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
}
protected Joint(JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
_type = def.type;
_bodyA = def.bodyA;
_bodyB = def.bodyB;
_collideConnected = def.collideConnected;
_userData = def.userData;
_edgeA = new JointEdge();
_edgeB = new JointEdge();
}
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
void SolveTOI(Body body)
{
// Find the minimum contact.
Contact toiContact = null;
float toi = 1.0f;
Body toiOther = null;
bool found;
int count;
int iter = 0;
bool bullet = body.IsBullet;
// Iterate until all contacts agree on the minimum TOI. We have
// to iterate because the TOI algorithm may skip some intermediate
// collisions when objects rotate through each other.
do
{
count = 0;
found = false;
for (ContactEdge ce = body._contactList; ce != null; ce = ce.Next)
{
if (ce.Contact == toiContact)
{
continue;
}
Body other = ce.Other;
BodyType type = other.GetType();
// Only bullets perform TOI with dynamic bodies.
if (bullet == true)
{
// Bullets only perform TOI with bodies that have their TOI resolved.
if ((other._flags & BodyFlags.Toi) == 0)
{
continue;
}
// No repeated hits on non-static bodies
if (type != BodyType.Static && (ce.Contact._flags & ContactFlags.BulletHit) != 0)
{
continue;
}
}
else if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
// Prevent infinite looping.
if (contact._toiCount > 10)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
int indexA = contact._indexA;
int indexB = contact._indexB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
Body bodyA = fixtureA._body;
Body bodyB = fixtureB._body;
// Compute the time of impact in interval [0, minTOI]
TOIInput input = new TOIInput();
input.proxyA.Set(fixtureA.GetShape(), indexA);
input.proxyB.Set(fixtureB.GetShape(), indexB);
input.sweepA = bodyA._sweep;
input.sweepB = bodyB._sweep;
input.tMax = toi;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, ref input);
if (output.State == TOIOutputState.Touching && output.t < toi)
{
toiContact = contact;
toi = output.t;
toiOther = other;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
body.Advance(1.0f);
return;
}
Sweep backup = body._sweep;
body.Advance(toi);
toiContact.Update(_contactManager.ContactListener);
if (toiContact.IsEnabled() == false)
{
// Contact disabled. Backup and recurse.
body._sweep = backup;
SolveTOI(body);
}
++toiContact._toiCount;
// Update all the valid contacts on this body and build a contact island.
count = 0;
for (ContactEdge ce = body._contactList; (ce != null) && (count < Settings.b2_maxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.GetType();
// Only perform correction with static bodies, so the
// body won't get pushed out of the world.
if (type == BodyType.Dynamic)
{
continue;
}
// Check for a disabled contact.
Contact contact = ce.Contact;
if (contact.IsEnabled() == false)
{
continue;
}
Fixture fixtureA = contact._fixtureA;
Fixture fixtureB = contact._fixtureB;
// Cull sensors.
if (fixtureA.IsSensor() || fixtureB.IsSensor())
{
continue;
}
// The contact likely has some new contact points. The listener
// gives the user a chance to disable the contact.
if (contact != toiContact)
{
contact.Update(_contactManager.ContactListener);
}
// Did the user disable the contact?
if (contact.IsEnabled() == false)
{
// Skip this contact.
continue;
}
if (contact.IsTouching() == false)
{
continue;
}
_toiContacts[count] = contact;
++count;
}
// Reduce the TOI body's overlap with the contact island.
_toiSolver.Initialize(_toiContacts, count, body);
float k_toiBaumgarte = 0.75f;
//bool solved = false;
for (int i = 0; i < 20; ++i)
{
bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte);
if (contactsOkay)
{
//solved = true;
break;
}
}
if (toiOther.GetType() != BodyType.Static)
{
toiContact._flags |= ContactFlags.BulletHit;
}
}
/// <summary> Create a joint to rain bodies together. No reference to the definition
/// is retained. This may cause the connected bodies to cease colliding.
/// </summary> <warning> This function is locked during callbacks.
/// </warning>
public Joint CreateJoint(JointDef def)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return(null);
}
Joint j = Joint.Create(def);
// Connect to the world list.
j._prev = null;
j._next = _jointList;
if (_jointList != null)
{
_jointList._prev = j;
}
_jointList = j;
++_jointCount;
// Connect to the bodies' doubly linked lists.
j._edgeA.Joint = j;
j._edgeA.Other = j._bodyB;
j._edgeA.Prev = null;
j._edgeA.Next = j._bodyA._jointList;
if (j._bodyA._jointList != null)
{
j._bodyA._jointList.Prev = j._edgeA;
}
j._bodyA._jointList = j._edgeA;
j._edgeB.Joint = j;
j._edgeB.Other = j._bodyA;
j._edgeB.Prev = null;
j._edgeB.Next = j._bodyB._jointList;
if (j._bodyB._jointList != null)
{
j._bodyB._jointList.Prev = j._edgeB;
}
j._bodyB._jointList = j._edgeB;
Body bodyA = def.bodyA;
Body bodyB = def.bodyB;
bool staticA = bodyA.GetType() == BodyType.Static;
bool staticB = bodyB.GetType() == BodyType.Static;
// If the joint prevents collisions, then flag any contacts for filtering.
if (def.collideConnected == false)
{
ContactEdge edge = bodyB.GetContactList();
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);
}
void Solve(ref TimeStep step)
{
// Size the island for the worst case.
_island.Reset(_bodyCount,
_contactManager._contactCount,
_jointCount,
_contactManager.ContactListener);
// Clear all the island flags.
for (Body b = _bodyList; b != null; b = b._next)
{
b._flags &= ~BodyFlags.Island;
}
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
c._flags &= ~ContactFlags.Island;
}
for (Joint j = _jointList; j != null; j = j._next)
{
j._islandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = _bodyCount;
if (stackSize > stack.Length)
{
stack = new Body[Math.Max(stack.Length * 2, stackSize)];
}
for (Body seed = _bodyList; seed != null; seed = seed._next)
{
if ((seed._flags & (BodyFlags.Island)) != BodyFlags.None)
{
continue;
}
if (seed.IsAwake() == false || seed.IsActive() == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.GetType() == BodyType.Static)
{
continue;
}
// Reset island and stack.
_island.Clear();
int stackCount = 0;
stack[stackCount++] = seed;
seed._flags |= BodyFlags.Island;
// Perform a depth first search (DFS) on the raint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = stack[--stackCount];
Debug.Assert(b.IsActive() == true);
_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.GetType() == BodyType.Static)
{
continue;
}
// Search all contacts connected to this body.
for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to an island?
if ((contact._flags & ContactFlags.Island) != ContactFlags.None)
{
continue;
}
// Is this contact solid and touching?
if (!ce.Contact.IsEnabled() || !ce.Contact.IsTouching())
{
continue;
}
// Skip sensors.
bool sensorA = contact._fixtureA._isSensor;
bool sensorB = contact._fixtureB._isSensor;
if (sensorA || sensorB)
{
continue;
}
_island.Add(contact);
contact._flags |= ContactFlags.Island;
Body other = ce.Other;
// Was the other body already added to this island?
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
// Search all joints connect to this body.
for (JointEdge je = b._jointList; je != null; je = je.Next)
{
if (je.Joint._islandFlag == true)
{
continue;
}
Body other = je.Other;
// Don't simulate joints connected to inactive bodies.
if (other.IsActive() == false)
{
continue;
}
_island.Add(je.Joint);
je.Joint._islandFlag = true;
if ((other._flags & BodyFlags.Island) != BodyFlags.None)
{
continue;
}
Debug.Assert(stackCount < stackSize);
stack[stackCount++] = other;
other._flags |= BodyFlags.Island;
}
}
_island.Solve(ref step, Gravity, _allowSleep);
// Post solve cleanup.
for (int i = 0; i < _island._bodyCount; ++i)
{
// Allow static bodies to participate in other islands.
Body b = _island._bodies[i];
if (b.GetType() == BodyType.Static)
{
b._flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
for (Body b = _bodyList; b != null; b = b.GetNext())
{
// If a body was not in an island then it did not move.
if ((b._flags & BodyFlags.Island) != BodyFlags.Island)
{
continue;
}
if (b.GetType() == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
_contactManager.FindNewContacts();
}
/// <summary> Call this to draw shapes and other debug draw data.
/// </summary>
public void DrawDebugData()
{
if (DebugDraw == null)
{
return;
}
DebugDrawFlags flags = DebugDraw.Flags;
if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == BodyType.Static)
{
DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == BodyType.Kinematic)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
{
Color color = new Color(0.3f, 0.9f, 0.9f);
for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
{
/*
* Fixture fixtureA = c.GetFixtureA();
* Fixture fixtureB = c.GetFixtureB();
*
* AABB aabbA;
* AABB aabbB;
* fixtureA.GetAABB(out aabbA);
* fixtureB.GetAABB(out aabbB);
*
* Vector2 cA = aabbA.GetCenter();
* Vector2 cB = aabbB.GetCenter();
*
* DebugDraw.DrawSegment(cA, cB, color);
*/
}
}
if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
BroadPhase bp = _contactManager._broadPhase;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if (b.IsActive() == false)
{
continue;
}
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
for (int i = 0; i < f._proxyCount; ++i)
{
FixtureProxy proxy = f._proxies[i];
AABB aabb;
bp.GetFatAABB(proxy.proxyId, out aabb);
FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>();
vs[0] = new Vector2(aabb.lowerBound.X, aabb.lowerBound.Y);
vs[1] = new Vector2(aabb.upperBound.X, aabb.lowerBound.Y);
vs[2] = new Vector2(aabb.upperBound.X, aabb.upperBound.Y);
vs[3] = new Vector2(aabb.lowerBound.X, aabb.upperBound.Y);
DebugDraw.DrawPolygon(ref vs, 4, color);
}
}
}
}
if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
xf.Position = b.GetWorldCenter();
DebugDraw.DrawTransform(ref xf);
}
}
}
public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep)
{
// Integrate velocities and apply damping.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
b._linearVelocity += step.dt * (gravity + b._invMass * b._force);
b._angularVelocity += step.dt * b._invI * 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
b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f);
b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f);
}
// Partition contacts so that contacts with static bodies are solved last.
int i1 = -1;
for (int i2 = 0; i2 < _contactCount; ++i2)
{
Fixture fixtureA = _contacts[i2].GetFixtureA();
Fixture fixtureB = _contacts[i2].GetFixtureB();
Body bodyA = fixtureA.GetBody();
Body bodyB = fixtureB.GetBody();
bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static;
if (nonStatic)
{
++i1;
//b2Swap(_contacts[i1], _contacts[i2]);
Contact temp = _contacts[i1];
_contacts[i1] = _contacts[i2];
_contacts[i2] = temp;
}
}
// Initialize velocity constraints.
_contactSolver.Reset(_contacts, _contactCount, step.dtRatio);
_contactSolver.WarmStart();
for (int i = 0; i < _jointCount; ++i)
{
_joints[i].InitVelocityConstraints(ref step);
}
// Solve velocity constraints.
for (int i = 0; i < step.velocityIterations; ++i)
{
for (int j = 0; j < _jointCount; ++j)
{
_joints[j].SolveVelocityConstraints(ref step);
}
_contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
_contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() == BodyType.Static)
{
continue;
}
// Check for large velocities.
Vector2 translation = step.dt * b._linearVelocity;
if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared)
{
float ratio = Settings.b2_maxTranslation / translation.Length();
b._linearVelocity *= ratio;
}
float rotation = step.dt * b._angularVelocity;
if (rotation * rotation > Settings.b2_maxRotationSquared)
{
float ratio = Settings.b2_maxRotation / Math.Abs(rotation);
b._angularVelocity *= ratio;
}
// Store positions for continuous collision.
b._sweep.c0 = b._sweep.c;
b._sweep.a0 = b._sweep.a;
// Integrate
b._sweep.c += step.dt * b._linearVelocity;
b._sweep.a += step.dt * b._angularVelocity;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < step.positionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte);
bool jointsOkay = true;
for (int j = 0; j < _jointCount; ++j)
{
bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
Report(_contactSolver._constraints);
if (allowSleep)
{
float minSleepTime = Settings.b2_maxFloat;
const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance;
const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance;
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
if (b.GetType() == BodyType.Static)
{
continue;
}
if ((b._flags & BodyFlags.AutoSleep) == 0)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b._flags & BodyFlags.AutoSleep) == 0 ||
b._angularVelocity * b._angularVelocity > angTolSqr ||
Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.b2_timeToSleep)
{
for (int i = 0; i < _bodyCount; ++i)
{
Body b = _bodies[i];
b.SetAwake(false);
}
}
}
}
public Atom(Symbol symbol, Vector2 position, GameContent gameContent, World world)
{
this.gameContent = gameContent;
this.world = world;
if (symbol == Symbol.Ra) eye = EyeState.Angry;
this.symbol = symbol;
this.symbolStr = symbol.ToString();
symbolCenter = gameContent.symbolFont.MeasureString(this.symbolStr);
symbolCenter.X *= 0.5f;
symbolCenter.Y *= 0.92f;
bondsLeft = (int)symbol;
BodyDef bd = new BodyDef();
bd.type = BodyType.Dynamic;
bd.position = this.position = position / gameContent.b2Scale;
bd.bullet = true;
body = world.CreateBody(bd);
body.SetUserData(this);
CircleShape cs = new CircleShape();
cs._radius = gameContent.atomRadius / gameContent.b2Scale;
FixtureDef fd = new FixtureDef();
fd.shape = cs;
fd.restitution = 0.2f;
fd.friction = 0.5f;
fixture = body.CreateFixture(fd);
electroShockAnimation = new Animation(gameContent.electroShock, 3, 0.1f, true, new Vector2(0.5f, 0.5f));
radiationSmoke = new RadiationSmoke(gameContent, this);
// Collide only with Ground but not with itself and bonded Filter
mouseFilter = new Filter();
mouseFilter.categoryBits = 0x0002; mouseFilter.maskBits = 0x0001; mouseFilter.groupIndex = -2;
// Collide with every thing
atomFilter = new Filter();
atomFilter.categoryBits = 0x0001; atomFilter.maskBits = 0x0001; atomFilter.groupIndex = 1;
fixture.SetFilterData(ref atomFilter);
SetMode(false, false);
}
// Sequentially solve TOIs for each body. We bring each
// body to the time of contact and perform some position correction.
// Time is not conserved.
void SolveTOI()
{
// Prepare all contacts.
for (Contact c = _contactManager._contactList; c != null; c = c._next)
{
// Enable the contact
c._flags |= ContactFlags.Enabled;
// Set the number of TOI events for this contact to zero.
c._toiCount = 0;
}
// Initialize the TOI flag.
for (Body body = _bodyList; body != null; body = body._next)
{
// Kinematic, and static bodies will not be affected by the TOI event.
// If a body was not in an island then it did not move.
if ((body._flags & BodyFlags.Island) == 0 || body.GetType() == BodyType.Kinematic || body.GetType() == BodyType.Static)
{
body._flags |= BodyFlags.Toi;
}
else
{
body._flags &= ~BodyFlags.Toi;
}
}
// Collide non-bullets.
for (Body body = _bodyList; body != null; body = body._next)
{
if ((body._flags & BodyFlags.Toi) != BodyFlags.None)
{
continue;
}
if (body.IsBullet == true)
{
continue;
}
SolveTOI(body);
body._flags |= BodyFlags.Toi;
}
// Collide bullets.
for (Body body = _bodyList; body != null; body = body._next)
{
if ((body._flags & BodyFlags.Toi) != BodyFlags.None)
{
continue;
}
if (body.IsBullet == false)
{
continue;
}
SolveTOI(body);
body._flags |= BodyFlags.Toi;
}
}