public static FixedPrismaticJoint CreateFixedPrismaticJoint(World world, Body body, Vector2 worldAnchor,
Vector2 axis)
{
FixedPrismaticJoint joint = new FixedPrismaticJoint(body, worldAnchor, axis);
world.AddJoint(joint);
return joint;
}
// Grab a rope
public void grabRope(int entityId, RopeComponent ropeComponent, Body ropeBody)
{
RopeGrabExclusionComponent ropeGrabExclusionComponent = EntityManager.getRopeGrabExclusionComponent(entityId);
if (ropeGrabExclusionComponent == null || !ropeGrabExclusionComponent.excludedRopeComponents.Contains(ropeComponent))
{
GroupComponent groupComponent = SystemManager.groupSystem.getGroupComponentContaining(entityId);
CharacterComponent characterComponent = EntityManager.getCharacterComponent(entityId);
int index = ropeComponent.bodies.IndexOf(ropeBody);
RevoluteJoint joint = JointFactory.CreateRevoluteJoint(SystemManager.physicsSystem.world, ropeBody, characterComponent.body, Vector2.Zero, Vector2.Zero);
RopeGrabComponent ropeGrabComponent = new RopeGrabComponent(entityId, ropeComponent, joint, (float)index);
EntityManager.addComponent(entityId, ropeGrabComponent);
if (groupComponent != null)
{
Formation activeFormation = groupComponent.activeFormation;
float maxPosition = ropeComponent.anchorBody.Position.X + 2f * groupComponent.entities.Count;
LimitedRangeFormation newFormation = new LimitedRangeFormation(groupComponent.entities, activeFormation.position, activeFormation.speed, float.MinValue, maxPosition);
ropeGrabComponent.formationToRemove = newFormation;
activeFormation.position = (ropeComponent.anchorBody.Position.X + maxPosition) * 0.5f;
groupComponent.addFormation(newFormation);
}
}
}
public IPhysical MakeCyllinder(double rbottom, double rtop, double height)
{
FBody fb = BodyFactory.CreateCircle(World, FarseerConverter.ToSimUnits((float)rbottom), (float)1);
fb.BodyType = BodyType.Dynamic;
return(new FarseerBody(fb));
}
public Agent(World world, Vector2 position)
{
_collidesWith = Category.All;
_collisionCategories = Category.All;
_agentBody = BodyFactory.CreateBody(world, position);
_agentBody.BodyType = BodyType.Dynamic;
//Center
FixtureFactory.AttachCircle(0.5f, 0.5f, _agentBody);
//Left arm
FixtureFactory.AttachRectangle(1.5f, 0.4f, 1f, new Vector2(-1f, 0f), _agentBody);
FixtureFactory.AttachCircle(0.5f, 0.5f, _agentBody, new Vector2(-2f, 0f));
//Right arm
FixtureFactory.AttachRectangle(1.5f, 0.4f, 1f, new Vector2(1f, 0f), _agentBody);
FixtureFactory.AttachCircle(0.5f, 0.5f, _agentBody, new Vector2(2f, 0f));
//Top arm
FixtureFactory.AttachRectangle(0.4f, 1.5f, 1f, new Vector2(0f, 1f), _agentBody);
FixtureFactory.AttachCircle(0.5f, 0.5f, _agentBody, new Vector2(0f, 2f));
//Bottom arm
FixtureFactory.AttachRectangle(0.4f, 1.5f, 1f, new Vector2(0f, -1f), _agentBody);
FixtureFactory.AttachCircle(0.5f, 0.5f, _agentBody, new Vector2(0f, -2f));
//GFX
_box = new Sprite(ContentWrapper.PolygonTexture(PolygonTools.CreateRectangle(1.75f, 0.2f), Color.White, ContentWrapper.Black));
_knob = new Sprite(ContentWrapper.CircleTexture(0.5f, "Square", ContentWrapper.Black, ContentWrapper.Gold, ContentWrapper.Black, 1f));
_offset = ConvertUnits.ToDisplayUnits(2f);
}
public static DistanceJoint CreateDistanceJoint(World world, Body bodyA, Body bodyB, Vector2 anchorA,
Vector2 anchorB)
{
DistanceJoint distanceJoint = new DistanceJoint(bodyA, bodyB, anchorA, anchorB);
world.AddJoint(distanceJoint);
return distanceJoint;
}
public static FixedDistanceJoint CreateFixedDistanceJoint(World world, Body body, Vector2 localAnchor,
Vector2 worldAnchor)
{
FixedDistanceJoint distanceJoint = new FixedDistanceJoint(body, localAnchor, worldAnchor);
world.AddJoint(distanceJoint);
return distanceJoint;
}
/// <summary>
/// Creates the fixed revolute joint.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="body">The body.</param>
/// <param name="bodyAnchor">The body anchor.</param>
/// <param name="worldAnchor">The world anchor.</param>
/// <returns></returns>
public static FixedRevoluteJoint CreateFixedRevoluteJoint(World world, Body body, Vector2 bodyAnchor,
Vector2 worldAnchor)
{
FixedRevoluteJoint fixedRevoluteJoint = new FixedRevoluteJoint(body, bodyAnchor, worldAnchor);
world.AddJoint(fixedRevoluteJoint);
return fixedRevoluteJoint;
}
public CoolRibbonObject(World world, Texture2D tex)
{
points = new List<Vector2>();
points.Add(new Vector2(4, -1));
points.Add(new Vector2(4, 22));
points.Add(new Vector2(30, 22));
points.Add(new Vector2(30, -1));
stops = new float[4];
for(int i = 0; i < points.Count-1; i++)
{
stops[i] = Vector2.Distance(points[i],points[i+1]);
}
body = BodyFactory.CreateBody(world);
body.BodyType = BodyType.Static;
body.Position = new Vector2(0,0);
body.UserData = this;
hinges = new List<Hinge>();
AddBox(world, tex);
ChainShape chain = new ChainShape(new Vertices(points));
fixture = body.CreateFixture(chain);
}
public PathTile(List<Vector2> localPoints, Vector2 position, World world, bool MakroCollision = false)
{
body = new Body(world);
body.Position = ConvertUnits.ToSimUnits(position);
body.UserData = "Wall";
body.IsStatic = true;
Vertices terrain = new Vertices();
foreach (Vector2 point in localPoints)
{
terrain.Add(ConvertUnits.ToSimUnits(point));
}
for (int i = 0; i < terrain.Count - 1; ++i)
{
FixtureFactory.AttachEdge(terrain[i], terrain[i + 1], body);
body.FixtureList[i].UserData = "Wall";
}
body.Restitution = 0f;
body.Friction = float.MaxValue;
if (!MakroCollision)
body.CollisionCategories = Category.Cat15 & ~Category.Cat3;
else
body.CollidesWith = Category.Cat29;
}
public void ScaleToBody(Body body)
{
Transform t;
body.GetTransform(out t);
AABB aabb;
aabb.LowerBound = new Vector2(Settings.MaxFloat, Settings.MaxFloat);
aabb.UpperBound = new Vector2(-Settings.MaxFloat, -Settings.MaxFloat);
AABB tempAABB;
int fixtureCount = body.FixtureList.Count;
for (int i = 0; i < fixtureCount; ++i)
{
body.FixtureList[i].Shape.ComputeAABB(out tempAABB, ref t, 0);
aabb.Combine(ref tempAABB);
}
Vector2 scale = new Vector2(
ConvertUnits.ToDisplayUnits(aabb.Width) / sourceRect.Width,
ConvertUnits.ToDisplayUnits(aabb.Height) / sourceRect.Height
);
this.scale = scale;
}
private void Explode()
{
if (Alive)
{
Vector2 explosionPoint = body.Position;
world.RemoveBody(body);
world.ProcessChanges();
body = null;
CuttingTools.Cut(world, explosionPoint + new Vector2(2, 2), explosionPoint - new Vector2(2, 2), 0.01f);
//world.ProcessChanges();
CuttingTools.Cut(world, explosionPoint + new Vector2(-2, 2), explosionPoint - new Vector2(-2, 2), 0.01f);
//world.ProcessChanges();
Explosion e = new Explosion(world);
e.Activate(explosionPoint, 2, 10);
Alive = false;
pivot = null;
Vector2 screenLoc = ProjectionHelper.FarseerToPixel(explosionPoint);
ParticleEffectManager.explosionEffect.Trigger(screenLoc);
}
}
public PinballMiniGame(GraphicsDevice graphicsDevice)
{
this.graphicsDevice = graphicsDevice;
sb = new SpriteBatch(graphicsDevice);
ein = new Kinect(0, 0);
ein.Init();
circleTexture = CreateCircle(ball_radius, graphicsDevice);
gameState = MiniGameState.Initialized;
physicsWorld = new World(new Vector2(0, .982f));
paddleSize = new Vector2(300, 16);
paddleLeft = BodyFactory.CreateRectangle(physicsWorld, paddleSize.X * 2 * pixelToUnit, paddleSize.Y * pixelToUnit, 1000f);
paddleLeft.BodyType = BodyType.Dynamic;
paddleLeft.Position = new Vector2(8 * pixelToUnit, (GameConstants.MiniGameCanvasHeight - 100) * pixelToUnit);
paddleLeft.LocalCenter = new Vector2(0, 0);
paddleLeft.SleepingAllowed = false;
paddleLeft.Restitution = 1.0f;
paddleLeft.IgnoreGravity = true;
ball = BodyFactory.CreateCircle(physicsWorld, (ball_radius) * pixelToUnit, 1000f);
ball.BodyType = BodyType.Dynamic;
ball.Position = new Vector2(100 * pixelToUnit, 100 * pixelToUnit);
ball.Restitution = 1.0f;
ball.SleepingAllowed = false;
}
public FarseerObject(FarseerWorld world, Shape shape, BodyType BodyType = BodyType.Dynamic)
{
body = new Body(world.World);
body.BodyType = BodyType;
body.CreateFixture(shape);
body.Enabled = false;
}
public void Add(Body body)
{
Debug.Assert(BodyCount < BodyCapacity);
body.IslandIndex = BodyCount;
Bodies[BodyCount] = body;
++BodyCount;
}
public static Vector2 CalculateOrigin(Body body)
{
Vector2 origin = Vector2.Zero;
if (body != null)
{
Vector2 leftBound = new Vector2(float.MaxValue);
Transform trans;
body.GetTransform(out trans);
for (int i = 0; i < body.FixtureList.Count; ++i)
{
for (int j = 0; j < body.FixtureList[i].Shape.ChildCount; ++j)
{
AABB bounds;
body.FixtureList[i].Shape.ComputeAABB(out bounds, ref trans, j);
Vector2.Min(ref leftBound, ref bounds.LowerBound, out leftBound);
}
}
// calculate body offset from its center and add a 1 pixel border
// because we generate the textures a little bigger than the actual body's fixtures
origin = ConvertUnits.ToDisplayUnits(body.Position - leftBound) + new Vector2(1f);
}
return origin;
}
private void SetupSnake(Body body, RubeScene scene)
{
Add(new SoundListener(body));
Add(new HeadController(body));
SetupTail(scene, body);
SetupControl(body);
}
public Player(World world, float x, float y, Texture2D texture)
{
torso = BodyFactory.CreateRectangle(world, 60 * MainGame.PIXEL_TO_METER, 80 * MainGame.PIXEL_TO_METER, 1);
torso.Position = new Vector2(x * MainGame.PIXEL_TO_METER, y * MainGame.PIXEL_TO_METER);
torso.BodyType = BodyType.Dynamic;
torso.UserData = this;
legs = BodyFactory.CreateCircle(world, 31 * MainGame.PIXEL_TO_METER, 1);
legs.Position = torso.Position + new Vector2(0, 40 * MainGame.PIXEL_TO_METER);
legs.BodyType = BodyType.Dynamic;
legs.Friction = 5.0f;
legs.UserData = this;
JointFactory.CreateFixedAngleJoint(world, torso);
axis = JointFactory.CreateRevoluteJoint(world, torso, legs, Vector2.Zero);
axis.CollideConnected = false;
axis.MotorEnabled = true;
axis.MotorSpeed = 0.0f;
axis.MotorTorque = 3.0f;
axis.MaxMotorTorque = 10.0f;
onGround = false;
facingLeft = false;
jumpForce = new Vector2(0, -1f);
rightAirForce = new Vector2(5f, 0);
leftAirForce = -1 * rightAirForce;
prevVelocity = Vector2.Zero;
normal = Vector2.Zero;
pressW = false;
holdW = false;
texIdle = new AnimatedTexture(texture, 24, 0, 0, 120, 140);
texRun = new AnimatedTexture(texture, 19, 0, 140, 120, 140);
texJump = new AnimatedTexture(texture, 9, 19 * 120, 140, 120, 140, 1, false, false);
currentTexture = texIdle;
}
private void CreateBorder(float width, float height, float borderWidth)
{
width = Math.Abs(width);
height = Math.Abs(height);
_anchor = new Body(_world);
List<Vertices> borders = new List<Vertices>(4);
//Bottom
borders.Add(PolygonTools.CreateRectangle(width, borderWidth, new Vector2(0f, height - borderWidth), 0));
//Left
borders.Add(PolygonTools.CreateRectangle(borderWidth, height, new Vector2(-width + borderWidth, 0), 0));
//Top
borders.Add(PolygonTools.CreateRectangle(width, borderWidth, new Vector2(0, -height + borderWidth), 0));
//Right
borders.Add(PolygonTools.CreateRectangle(borderWidth, height, new Vector2(width - borderWidth, 0), 0));
RenderMaterial material = new RenderMaterial(_texture, _textureName)
{
Color = _color,
Scale = 8f
};
List<Fixture> fixtures = FixtureFactory.CreateCompoundPolygon(borders, 1, _anchor, material);
foreach (Fixture t in fixtures)
{
t.CollisionFilter.CollisionCategories = Category.All;
t.CollisionFilter.CollidesWith = Category.All;
}
}
public override bool IsActiveOn(Body body)
{
if (body.ControllerFilter.IsControllerIgnored(_type))
return false;
return base.IsActiveOn(body);
}
public PhysicsGameEntity(Game game, World world, Category collisionCategory, Body body, Vector2 origin)
: this(game,world,collisionCategory)
{
Body = body;
Center = origin;
Body.CollisionCategories = collisionCategory;
}
public void AddSolid()
{
if (!Dead)
{
_floor = BodyFactory.CreateRectangle(_world, ConvertUnits.ToSimUnits(Globals.SmallGridSize.X),
ConvertUnits.ToSimUnits(Globals.SmallGridSize.Y), 30f);
_floor.Position = ConvertUnits.ToSimUnits(_position.X + _centerVect.X, _position.Y + _centerVect.Y);
_floor.IsStatic = true;
_floor.Restitution = _stats.Restitution;
_floor.Friction = _stats.Friction;
//_bullet.LinearDamping = 0.2f;
//_bullet.AngularDamping = 0.2f;
_floor.Enabled = false;
_floor.UserData = "caveblock";
hull = ShadowHull.CreateRectangle(new Vector2(Globals.SmallGridSize.X, Globals.SmallGridSize.Y));
hull.Position.X = _position.X + _centerVect.X;
hull.Position.Y = _position.Y + _centerVect.Y;
krypton.Hulls.Add(hull);
hull.Visible = false;
SSolid = true;
GoingtobeSolid = false;
}
}
/// <summary>
/// Creates a revolute joint and adds it to the world
/// </summary>
/// <param name="world"></param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <param name="anchorB"></param>
/// <returns></returns>
public static RevoluteJoint CreateRevoluteJoint(World world, Body bodyA, Body bodyB, Vector2 anchorB)
{
Vector2 localanchorA = bodyA.GetLocalPoint(bodyB.GetWorldPoint(anchorB));
RevoluteJoint joint = new RevoluteJoint(bodyA, bodyB, localanchorA, anchorB);
world.AddJoint(joint);
return joint;
}
public static void AttachOnCollisionHandlers(Body body, EasyTopDownGame game)
{
foreach (Fixture fixture in body.FixtureList)
{
fixture.OnCollision += game.OnFixtureCollision;
}
}
/// <summary>
/// Creates an angle joint.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <returns></returns>
public static AngleJoint CreateAngleJoint(World world, Body bodyA, Body bodyB)
{
AngleJoint angleJoint = new AngleJoint(bodyA, bodyB);
world.AddJoint(angleJoint);
return angleJoint;
}
public override Vector2 getDirection(SpriteObjects.Ship mShip, Body otherShipsBody, float directionWeight)
{
float rearRayLength = mShip.getRayCastLengths.ElementAt(4); //length of raycast at the back of the ship
float frontRayLength = mShip.getRayCastLengths.ElementAt(0); //length of raycast at the front of the ship
Vector2 thrust = new Vector2(0, 99);
//if (otherShipsBody == null || mShip.mSpriteBody == null)
//{
// thrust = thrust;
//}
//else if (otherShipsBody.Position.Equals(mShip.mSpriteBody.Position))
//{
// thrust = Vector2.Zero;
//}
if (rearRayLength < (-SpriteObjects.Ship.BASE_SHIP_RAYCAST_LENGTH))
{
thrust = new Vector2(0, -directionWeight);
}
//else
//{
// thrust = Vector2.Zero;
//}
if ((frontRayLength < (-SpriteObjects.Ship.BASE_SHIP_RAYCAST_LENGTH)))
{
thrust = Vector2.Zero;
}
return thrust;
}
/// <summary>
/// Creates a fixed angle joint.
/// </summary>
/// <param name="world">The world.</param>
/// <param name="body">The body.</param>
/// <returns></returns>
public static FixedAngleJoint CreateFixedAngleJoint(World world, Body body)
{
FixedAngleJoint angleJoint = new FixedAngleJoint(body);
world.AddJoint(angleJoint);
return angleJoint;
}
public Border(World _world)
{
_anchorBottom = BodyFactory.CreateLineArc(_world, 2* MathHelper.Pi, 100, 100, position: new Vector2 (40f, 36f));
_anchorTop = BodyFactory.CreateLineArc(_world, MathHelper.Pi, 100, 100, position: new Vector2(40f, 36f), rotation: MathHelper.Pi);
_exitBottom = BodyFactory.CreateLineArc(_world, 2 * MathHelper.Pi, 100, 110, position: new Vector2(40f, 36f));
_exitTop = BodyFactory.CreateLineArc(_world, MathHelper.Pi, 100, 110, position: new Vector2(40f, 36f), rotation: MathHelper.Pi);
_exitTop.OnCollision += OnCollision;
_exitBottom.OnCollision += OnCollision;
_exitTop.CollisionCategories = Category.All;
_exitTop.CollidesWith = Category.All;
_exitBottom.CollisionCategories = Category.All;
_exitBottom.CollidesWith = Category.All;
_anchorTop.CollisionCategories = Category.Cat5; //.All & ~Category.Cat3 & ~Category.Cat2;
_anchorTop.CollidesWith = Category.Cat1;//Category.All & ~Category.Cat3 & ~Category.Cat2; //collides with enemy and player
_anchorBottom.CollisionCategories = Category.All & ~Category.Cat3;
_anchorBottom.CollidesWith = Category.Cat1;//Category.All & ~Category.Cat3 & ~Category.Cat2; //collides with enemy and player
//_anchorTop.IsSensor = true;
//_anchorTop.ContactList
//_anchorBottom.OnSeparation += OnSeparation;
//System.Diagnostics.Debug.WriteLine( _anchorTop.ContactList+"contact list");
}
/// <summary>
///
/// </summary>
/// <param name="type"></param>
/// <param name="disable"></param>
/// <param name="cooldown"></param>
public TriggerEntity_cl(string type, bool disable = false, float cooldown = 0.0f)
: base()
{
mDisableAfterTrigger = disable;
mCooldownTime = cooldown;
mCooldownTimer = 0.0f;
AddComponent(new PositionComponent_cl(this));
AddComponent(new RenderableComponent_cl(this, "whitePixel"));
((RenderableComponent_cl)GetComponentOfType(typeof(RenderableComponent_cl))).Sprite.Size = new Vector2(TRIGGER_DEFAULT_SIZE, TRIGGER_DEFAULT_SIZE);
mPhysicsBody = BodyFactory.CreateBody(PhysicsManager_cl.Instance.PhysicsWorld);
SetShape(PhysicsComponent_cl.PhysicsObjectType.RECTANGLE);
/************************************************************************
* TODO:
* Allow multiple trigger types.
* Ex: Sound and DamageHealth components on a single TriggerEntity
*
* Jay Sternfield - 2011/12/05
************************************************************************/
AddTriggerOfType(type);
TriggerManager_cl.Instance.AddTrigger(this);
mEnabled = true;
}
/// <summary>
///
/// </summary>
/// <param name="body"></param>
/// <returns></returns>
public virtual bool IsActiveOn(Body body)
{
if (body == null || !body.Enabled || body.IsStatic)
return false;
if (body.FixtureList == null)
return false;
foreach (Fixture fixture in body.FixtureList)
{
//Disable
if ((fixture.CollisionGroup == DisabledOnGroup) && fixture.CollisionGroup != 0 && DisabledOnGroup != 0)
return false;
if ((fixture.CollisionCategories & DisabledOnCategories) != Category.None)
return false;
if (EnabledOnGroup != 0 || EnabledOnCategories != Category.All)
{
//Enable
if ((fixture.CollisionGroup == EnabledOnGroup) && fixture.CollisionGroup != 0 && EnabledOnGroup != 0)
return true;
if ((fixture.CollisionCategories & EnabledOnCategories) != Category.None &&
EnabledOnCategories != Category.All)
return true;
}
else
{
return true;
}
}
return false;
}
protected override Joint CreateJoint(Body bodyA, Body bodyB)
{
if (bodyA == null) return null;
FixedMouseJoint mj = new FixedMouseJoint(bodyA, Vector2.Zero);
Scene.PhysicsWorld.AddJoint(mj);
return mj;
}
public void DrawBody(Body body, Color color)
{
// shift the current context into place to stamp down the little critter
basicEffect.World = Matrix.Multiply(Matrix.CreateRotationZ(body.Rotation), Matrix.CreateTranslation(new Vector3(body.Position, 0.0f)));
basicEffect.CurrentTechnique.Passes[0].Apply();
// go through every fixture in this body (only one atm: the square!)
foreach (Fixture fixture in body.FixtureList)
{
/*
* cast the shape into a special case of the Shape object -> PolygonShape. PolygonShape knows all about Vertices, which we need to draw. There
* are a whole host of other shape types that we could draw in different ways
*/
PolygonShape shape = (PolygonShape)fixture.Shape;
// create a new vertex array to hold our drawing information (one bigger than the number of Vertices to loop back round and close the rectangle
VertexPositionColor[] vertices = new VertexPositionColor[shape.Vertices.Count + 1];
// loop through the vertices, saving each one into an array slot
int i = 0;
foreach (Vector2 vector in shape.Vertices)
{
vertices[i].Position = new Vector3(vector, 0.0f);
vertices[i].Color = color;
++i;
}
// put in the final vertex, closing off the rectangle
vertices[i].Position = new Vector3(shape.Vertices[0], 0.0f);
vertices[i].Color = color;
// draw the rectangle to the screen
graphics.DrawUserPrimitives<VertexPositionColor>(PrimitiveType.LineStrip, vertices, 0, 4);
}
}
//--------------------------------------------------------------------------
#region Making primitives
public IPhysical MakeBox(double xsize, double ysize, double zsise)
{
FBody fb = BodyFactory.CreateRectangle(World,
FarseerConverter.ToSimUnits((float)xsize), FarseerConverter.ToSimUnits((float)ysize), (float)1);
fb.BodyType = BodyType.Dynamic;
return(new FarseerBody(fb));
}
public Body(FarseerPhysics.Dynamics.World farseerWorld, GameObject gameObject)
{
this._farseerWorld = farseerWorld;
this._body = new FarseerPhysics.Dynamics.Body(
this._farseerWorld,
Vector2.Zero,
0f,
IsRigid ? BodyType.Dynamic : BodyType.Static,
gameObject
);
}
public Body(Vector2?position = null, Dyn.BodyType bodyType = Dyn.BodyType.Dynamic) : base()
{
Shape = Body.RectShape(10, 20);
BodyType = bodyType;
var pos = position ?? Vector2.Zero;
Xna.Vector2 xpos = Util.ConvertVector2(pos);
PBody = new Dyn.Body(DummyWorld, position: xpos, userdata: this);
Fixture = PBody.CreateFixture(Shape, userData: this);
PBody.BodyType = BodyType;
}
void rotate_body_to(FarseerPhysics.Dynamics.Body body, float desiredAngle)
{
float DEGTORAD = Mathf.PI / 180f;
float bodyAngle = body.Rotation;
float nextAngle = bodyAngle + body.AngularVelocity * Time.deltaTime;
float totalRotation = desiredAngle - nextAngle;
while (totalRotation < -180 * DEGTORAD)
{
totalRotation += 360 * DEGTORAD;
}
while (totalRotation > 180 * DEGTORAD)
{
totalRotation -= 360 * DEGTORAD;
}
float desiredAngularVelocity = totalRotation * 60;
float impulse = body.Inertia * desiredAngularVelocity; // disregard time factor
body.ApplyAngularImpulse(impulse / 5f);
}
public override void Initialize()
{
// Reset physics world
Vector2 Gravity = new Vector2(0f, 0f);
World = new World(Gravity);
// Reset containers
Units = new List <Unit>();
// Reset camera
Game.Camera.Position = new Vector2(-Game.Graphics.PreferredBackBufferWidth / 2f, -Game.Graphics.PreferredBackBufferHeight / 2f);
Game.Camera.Zoom = 1f;
StartRay = new Vector2(-200f, -200f);
LastCollisionPosition = Vector2.Zero;
// Test Obstacle
PhysicsBody Obstacle = BodyFactory.CreateRectangle(World, ConvertUnits.ToSimUnits(200f), ConvertUnits.ToSimUnits(200f), 1f);
Obstacle.Position = ConvertUnits.ToSimUnits(new Vector2(0f, 0f));
Obstacle.FixtureList[0].IsSensor = true;
// Test Unit
Sprite S = new Sprite(Game.Sprites["Cat"]);
Unit = new Unit("Collision Avoid", S, World);
Unit.DrawDebugVelocity = false;
Unit.Body.MaxVelocity = 100f;
Unit.Transform.Position = new Vector2(-500f, 0f);
Unit.Collider.Body.Position = ConvertUnits.ToSimUnits(Unit.Transform.Position);
Unit.AddSteering(SteeringType.Wander);
Unit.AddSteering(SteeringType.ObstacleAvoidance);
Unit.SetSteeringWeight(SteeringType.ObstacleAvoidance, 100);
Units.Add(Unit);
}
public void Solve(ref TimeStep step, ref Vector2 gravity)
{
// Integrate velocities and apply damping.
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// Integrate velocities.
// FPE 3 only - Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.X += step.dt * (b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (b.InvMass * b.Force.Y);
b.AngularVelocityInternal += step.dt * b.InvI * b.Torque;
}
else
{
b.LinearVelocityInternal.X += step.dt * (gravity.X + b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += step.dt * (gravity.Y + b.InvMass * b.Force.Y);
b.AngularVelocityInternal += 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.LinearVelocityInternal *= MathUtils.Clamp(1.0f - step.dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= 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 < this.ContactCount; ++i2)
{
Fixture fixtureA = this._contacts[i2].FixtureA;
Fixture fixtureB = this._contacts[i2].FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
bool nonStatic = bodyA.BodyType != BodyType.Static && bodyB.BodyType != BodyType.Static;
if (nonStatic)
{
++i1;
//TODO: Only swap if they are not the same? see http://code.google.com/p/box2d/issues/detail?id=162
Contact tmp = this._contacts[i1];
this._contacts[i1] = this._contacts[i2];
this._contacts[i2] = tmp;
}
}
// Initialize velocity constraints.
this._contactSolver.Reset(this._contacts, this.ContactCount, step.dtRatio, Settings.EnableWarmstarting);
this._contactSolver.InitializeVelocityConstraints();
#pragma warning disable 0162 // Unreachable code detected
if (Settings.EnableWarmstarting)
{
this._contactSolver.WarmStart();
}
#pragma warning restore 0162 // Unreachable code detected
if (Settings.EnableDiagnostics)
{
this._watch.Start();
this._tmpTime = 0;
}
for (int i = 0; i < this.JointCount; ++i)
{
if (this._joints[i].Enabled)
{
this._joints[i].InitVelocityConstraints(ref step);
}
}
if (Settings.EnableDiagnostics)
{
this._tmpTime += this._watch.ElapsedTicks;
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
if (Settings.EnableDiagnostics)
{
this._watch.Start();
}
for (int j = 0; j < this.JointCount; ++j)
{
Joint joint = this._joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref step);
joint.Validate(step.inv_dt);
}
if (Settings.EnableDiagnostics)
{
this._watch.Stop();
this._tmpTime += this._watch.ElapsedTicks;
this._watch.Reset();
}
this._contactSolver.SolveVelocityConstraints();
}
// Post-solve (store impulses for warm starting).
this._contactSolver.StoreImpulses();
// Integrate positions.
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationX = step.dt * b.LinearVelocityInternal.X;
float translationY = step.dt * b.LinearVelocityInternal.Y;
float result = translationX * translationX + translationY * translationY;
if (result > Settings.MaxTranslationSquared)
{
float sq = (float)Math.Sqrt(result);
float ratio = Settings.MaxTranslation / sq;
b.LinearVelocityInternal.X *= ratio;
b.LinearVelocityInternal.Y *= ratio;
}
float rotation = step.dt * b.AngularVelocityInternal;
if (rotation * rotation > Settings.MaxRotationSquared)
{
float ratio = Settings.MaxRotation / Math.Abs(rotation);
b.AngularVelocityInternal *= ratio;
}
// Store positions for continuous collision.
b.Sweep.C0.X = b.Sweep.C.X;
b.Sweep.C0.Y = b.Sweep.C.Y;
b.Sweep.A0 = b.Sweep.A;
// Integrate
b.Sweep.C.X += step.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += step.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += step.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
// Iterate over constraints.
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = this._contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
bool jointsOkay = true;
if (Settings.EnableDiagnostics)
{
this._watch.Start();
}
for (int j = 0; j < this.JointCount; ++j)
{
Joint joint = this._joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints();
jointsOkay = jointsOkay && jointOkay;
}
if (Settings.EnableDiagnostics)
{
this._watch.Stop();
this._tmpTime += this._watch.ElapsedTicks;
this._watch.Reset();
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
break;
}
}
if (Settings.EnableDiagnostics)
{
this.JointUpdateTime = this._tmpTime;
}
Report(this._contactSolver.Constraints);
if (Settings.AllowSleep)
{
float minSleepTime = Settings.MaxFloat;
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
Vector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += step.dt;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep)
{
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
b.Awake = false;
}
}
}
}
public Fixture(Body body, Shape shape)
: this(body, shape, null)
{
}
public Fixture DeepClone()
{
Fixture fix = Clone(Body.Clone());
return(fix);
}
public override void Initialize()
{
// Reset physics world
Vector2 Gravity = new Vector2(0f, 0f);
World = new World(Gravity);
// Reset containers
Units = new List <Unit>();
// Reset camera
Game.Camera.Position = new Vector2(-Game.Graphics.PreferredBackBufferWidth / 2f, -Game.Graphics.PreferredBackBufferHeight / 2f);
Game.Camera.Zoom = 1f;
// Load map and its renderer
Map = Game.Maps["MainMap"];
MapRenderer = new TiledMapRenderer(Game.GraphicsDevice);
// Generate map obstacles
if (Map.ObjectLayers.Count != 0)
{
foreach (TiledMapObject O in Map.ObjectLayers[0].Objects)
{
if (O.Type == "Rectangle" && O.Name.Length == 0)
{
// Create a static body in the physics world
PhysicsBody B = BodyFactory.CreateRectangle(World, ConvertUnits.ToSimUnits(O.Size.Width), ConvertUnits.ToSimUnits(O.Size.Height), 1f);
Vector2 Pos = new Vector2(O.Position.X + O.Size.Width / 2f, O.Position.Y + O.Size.Height / 2f);
B.Position = ConvertUnits.ToSimUnits(Pos);
B.FixtureList[0].IsSensor = true;
}
}
}
// Create the pathfinding grid based on the tilemap Height, Width and TileHeight
PathfindingGrid = new Grid(Map);
Pathfinder = new Pathfinding(PathfindingGrid);
Manager = new GameManager();
SelectedUnit = null;
Selecting = false;
// Debug
DrawDebugGrid = false;
DebugGridTexture = new Texture2D(Game.GraphicsDevice, 1, 1);
DebugGridTexture.SetData(new Color[] { Color.White });
// Setup Game
Sprite Stump = new Sprite(Game.Content.Load <Texture2D>("Sprites//Stump"));
Sprite Ghost = new Sprite(Game.Content.Load <Texture2D>("Sprites//Ghost"));
Sprite Cat = new Sprite(Game.Content.Load <Texture2D>("Sprites//Cat"));
Sprite Healer = new Sprite(Game.Content.Load <Texture2D>("Sprites//Healer"));
Sprite Assassin = new Sprite(Game.Content.Load <Texture2D>("Sprites//Assassin"));
CreateAssassin("Blue Assassin", Assassin, Team.Blue, new Vector2(800f, 800f));
CreateMelee("Blue Melee", Cat, Team.Blue, new Vector2(600f, 800f));
CreateRanged("Red Ranged", Ghost, Team.Red, new Vector2(2000f, 1800f));
// Generate Units
if (Map.ObjectLayers.Count != 0)
{
foreach (TiledMapObject O in Map.ObjectLayers[0].Objects)
{
if (O.Type == "Point" && O.Name.Contains("Leader"))
{
if (O.Name == "Red Leader")
{
CreateLeader(O.Name, Healer, Team.Red, O.Position);
Manager.RedTeamBase = O.Position;
}
else if (O.Name == "Blue Leader")
{
CreateLeader(O.Name, Stump, Team.Blue, O.Position);
Manager.BlueTeamBase = O.Position;
}
}
else if (O.Type == "Rectangle" && O.Name.Contains("Healing"))
{
if (O.Name == "Red Healing")
{
Manager.RedHealingPoint = new RectangleF(O.Position.X, O.Position.Y, O.Size.Width, O.Size.Height);
}
else if (O.Name == "Blue Healing")
{
Manager.BlueHealingPoint = new RectangleF(O.Position.X, O.Position.Y, O.Size.Width, O.Size.Height);
}
}
}
}
// Set the references to the Leader in the GameManager
foreach (Unit U in Units)
{
if (U.Name == "Red Leader")
{
Manager.RedLeader = (LeaderUnit)U;
}
else if (U.Name == "Blue Leader")
{
Manager.BlueLeader = (LeaderUnit)U;
}
}
// TODO: Assign a reference of the GameManager to each Unit so they can access its info
}
public override void Initialize()
{
// Reset physics world
Vector2 Gravity = new Vector2(0f, 0f);
World = new World(Gravity);
// Reset containers
Units = new List <Unit>();
// Reset camera
Game.Camera.Position = new Vector2(-300f, -150f);
Game.Camera.Zoom = 1.5f;
// Load map and its renderer
Map = Game.Maps["PathfindingMap"];
MapRenderer = new TiledMapRenderer(Game.GraphicsDevice);
// Generate map obstacles
if (Map.ObjectLayers.Count != 0)
{
foreach (TiledMapObject O in Map.ObjectLayers[0].Objects)
{
// Create a static body in the physics world
PhysicsBody B = BodyFactory.CreateRectangle(World, ConvertUnits.ToSimUnits(O.Size.Width), ConvertUnits.ToSimUnits(O.Size.Height), 1f);
Vector2 Pos = new Vector2(O.Position.X + O.Size.Width / 2f, O.Position.Y + O.Size.Height / 2f);
B.Position = ConvertUnits.ToSimUnits(Pos);
B.FixtureList[0].IsSensor = true;
}
}
// Create the pathfinding grid based on the tilemap Height, Width and TileHeight
PathfindingGrid = new Grid(Map);
Pathfinder = new Pathfinding(PathfindingGrid);
End = new Vector2(400f, 240f);
Path = new Path();
// Test Unit
Sprite S = new Sprite(Game.Sprites["Ghost"]);
Unit = new Unit("Pathfinder", S, World);
Unit.Transform.Position = new Vector2(50f, 50f);
Unit.Collider.Body.Position = ConvertUnits.ToSimUnits(Unit.Transform.Position);
Unit.Transform.Scale = new Vector2(0.5f, 0.5f);
Unit.Body.MaxVelocity = 50f;
Unit.AddSteering(SteeringType.PathFollowing);
Path = Pathfinder.FindPath(Unit.Transform.Position, End);
Unit.Move(Path);
Units.Add(Unit);
// Debug
DrawDebugGrid = true;
DebugGridTexture = new Texture2D(Game.GraphicsDevice, 1, 1);
DebugGridTexture.SetData(new Color[] { Color.White });
// Wander Unit
Unit2 = new Unit("Wanderer", S, World);
Unit2.Transform.Position = new Vector2(100f, 100f);
Unit2.Collider.Body.Position = ConvertUnits.ToSimUnits(Unit2.Transform.Position);
Unit2.Transform.Scale = new Vector2(0.5f, 0.5f);
Unit2.Body.MaxVelocity = 50f;
Unit2.AddSteering(SteeringType.Wander);
Unit2.AddSteering(SteeringType.ObstacleAvoidance);
Unit2.SetSteeringWeight(SteeringType.ObstacleAvoidance, 100);
Units.Add(Unit2);
}
/// <summary>
/// Find TOI contacts and solve them.
/// </summary>
/// <param name="step">The step.</param>
private void SolveTOI(ref TimeStep step)
{
this.Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, this.ContactManager);
if (this._stepComplete)
{
for (int i = 0; i < this.BodyList.Count; i++)
{
this.BodyList[i].Flags &= ~BodyFlags.Island;
this.BodyList[i].Sweep.Alpha0 = 0.0f;
}
for (int i = 0; i < this.ContactManager.ContactList.Count; i++)
{
Contact c = this.ContactManager.ContactList[i];
// Invalidate TOI
c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
c.TOICount = 0;
c.TOI = 1.0f;
}
}
// Find TOI events and solve them.
for (; ;)
{
// Find the first TOI.
Contact minContact = null;
float minAlpha = 1.0f;
for (int i = 0; i < this.ContactManager.ContactList.Count; i++)
{
Contact c = this.ContactManager.ContactList[i];
// Is this contact disabled?
if (c.Enabled == false)
{
continue;
}
// Prevent excessive sub-stepping.
if (c.TOICount > Settings.MaxSubSteps)
{
continue;
}
float alpha;
if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI)
{
// This contact has a valid cached TOI.
alpha = c.TOI;
}
else
{
Fixture fA = c.FixtureA;
Fixture fB = c.FixtureB;
// Is there a sensor?
if (fA.IsSensor || fB.IsSensor)
{
continue;
}
Body bA = fA.Body;
Body bB = fB.Body;
BodyType typeA = bA.BodyType;
BodyType typeB = bB.BodyType;
Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic);
bool awakeA = bA.Awake && typeA != BodyType.Static;
bool awakeB = bB.Awake && typeB != BodyType.Static;
// Is at least one body awake?
if (awakeA == false && awakeB == false)
{
continue;
}
bool collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && !bA.IgnoreCCD;
bool collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && !bB.IgnoreCCD;
// 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);
}
Debug.Assert(alpha0 < 1.0f);
// Compute the time of impact in interval [0, minTOI]
this._input.ProxyA.Set(fA.Shape, c.ChildIndexA);
this._input.ProxyB.Set(fB.Shape, c.ChildIndexB);
this._input.SweepA = bA.Sweep;
this._input.SweepB = bB.Sweep;
this._input.TMax = 1.0f;
TOIOutput output;
TimeOfImpact.CalculateTimeOfImpact(out output, this._input);
// Beta is the fraction of the remaining portion of the .
float beta = output.T;
if (output.State == TOIOutputState.Touching)
{
alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
}
else
{
alpha = 1.0f;
}
c.TOI = alpha;
c.Flags |= ContactFlags.TOI;
}
if (alpha < minAlpha)
{
// This is the minimum TOI found so far.
minContact = c;
minAlpha = alpha;
}
}
if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha)
{
// No more TOI events. Done!
this._stepComplete = true;
break;
}
// Advance the bodies to the TOI.
Fixture fA1 = minContact.FixtureA;
Fixture fB1 = minContact.FixtureB;
Body bA1 = fA1.Body;
Body bB1 = fB1.Body;
Sweep backup1 = bA1.Sweep;
Sweep backup2 = bB1.Sweep;
bA1.Advance(minAlpha);
bB1.Advance(minAlpha);
// The TOI contact likely has some new contact points.
minContact.Update(this.ContactManager);
minContact.Flags &= ~ContactFlags.TOI;
++minContact.TOICount;
// Is the contact solid?
if (minContact.Enabled == false || minContact.IsTouching() == false)
{
// Restore the sweeps.
minContact.Enabled = false;
bA1.Sweep = backup1;
bB1.Sweep = backup2;
bA1.SynchronizeTransform();
bB1.SynchronizeTransform();
continue;
}
bA1.Awake = true;
bB1.Awake = true;
// Build the island
this.Island.Clear();
this.Island.Add(bA1);
this.Island.Add(bB1);
this.Island.Add(minContact);
bA1.Flags |= BodyFlags.Island;
bB1.Flags |= BodyFlags.Island;
minContact.Flags |= ContactFlags.Island;
// Get contacts on bodyA and bodyB.
Body[] bodies = { bA1, bB1 };
for (int i = 0; i < 2; ++i)
{
Body body = bodies[i];
if (body.BodyType == BodyType.Dynamic)
{
// for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next)
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
Contact contact = ce.Contact;
// Has this contact already been added to the island?
if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island)
{
continue;
}
// Only add static, kinematic, or bullet bodies.
Body other = ce.Other;
if (other.BodyType == BodyType.Dynamic &&
body.IsBullet == false && other.IsBullet == false)
{
continue;
}
// Skip sensors.
if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor)
{
continue;
}
// Tentatively advance the body to the TOI.
Sweep backup = other.Sweep;
if ((other.Flags & BodyFlags.Island) == 0)
{
other.Advance(minAlpha);
}
// Update the contact points
contact.Update(this.ContactManager);
// Was the contact disabled by the user?
if (contact.Enabled == 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.Flags |= ContactFlags.Island;
this.Island.Add(contact);
// Has the other body already been added to the island?
if ((other.Flags & BodyFlags.Island) == BodyFlags.Island)
{
continue;
}
// Add the other body to the island.
other.Flags |= BodyFlags.Island;
if (other.BodyType != BodyType.Static)
{
other.Awake = true;
}
this.Island.Add(other);
}
}
}
TimeStep 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;
this.Island.SolveTOI(ref subStep);
// Reset island flags and synchronize broad-phase proxies.
for (int i = 0; i < this.Island.BodyCount; ++i)
{
Body body = this.Island.Bodies[i];
body.Flags &= ~BodyFlags.Island;
if (body.BodyType != BodyType.Dynamic)
{
continue;
}
body.SynchronizeFixtures();
// Invalidate all contact TOIs on this displaced body.
for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next)
{
ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island);
}
}
// Commit fixture proxy movements to the broad-phase so that new contacts are created.
// Also, some contacts can be destroyed.
this.ContactManager.FindNewContacts();
if (this.EnableSubStepping)
{
this._stepComplete = false;
break;
}
}
}
private void ProcessAddedJoints()
{
if (this._jointAddList.Count > 0)
{
foreach (Joint joint in this._jointAddList)
{
// Connect to the world list.
this.JointList.Add(joint);
// Connect to the bodies' doubly linked lists.
joint.EdgeA.Joint = joint;
joint.EdgeA.Other = joint.BodyB;
joint.EdgeA.Prev = null;
joint.EdgeA.Next = joint.BodyA.JointList;
if (joint.BodyA.JointList != null)
{
joint.BodyA.JointList.Prev = joint.EdgeA;
}
joint.BodyA.JointList = joint.EdgeA;
joint.EdgeB.Joint = joint;
joint.EdgeB.Other = joint.BodyA;
joint.EdgeB.Prev = null;
joint.EdgeB.Next = joint.BodyB.JointList;
if (joint.BodyB.JointList != null)
{
joint.BodyB.JointList.Prev = joint.EdgeB;
}
joint.BodyB.JointList = joint.EdgeB;
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (joint.CollideConnected == false)
{
ContactEdge 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;
}
}
if (this.JointAdded != null)
{
this.JointAdded(joint);
}
// Note: creating a joint doesn't wake the bodies.
}
this._jointAddList.Clear();
}
}
public void Dispose()
{
_farseerWorld.RemoveBody(this._body);
this._body = null;
}
internal void Destroy(Contact contact)
{
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
if (EndContact != null && contact.IsTouching())
{
EndContact(contact);
}
// Remove from the world.
if (contact.Prev != null)
{
contact.Prev.Next = contact.Next;
}
if (contact.Next != null)
{
contact.Next.Prev = contact.Prev;
}
if (contact == ContactList)
{
ContactList = contact.Next;
}
// Remove from body 1
if (contact.NodeA.Prev != null)
{
contact.NodeA.Prev.Next = contact.NodeA.Next;
}
if (contact.NodeA.Next != null)
{
contact.NodeA.Next.Prev = contact.NodeA.Prev;
}
if (contact.NodeA == bodyA.ContactList)
{
bodyA.ContactList = contact.NodeA.Next;
}
// Remove from body 2
if (contact.NodeB.Prev != null)
{
contact.NodeB.Prev.Next = contact.NodeB.Next;
}
if (contact.NodeB.Next != null)
{
contact.NodeB.Next.Prev = contact.NodeB.Prev;
}
if (contact.NodeB == bodyB.ContactList)
{
bodyB.ContactList = contact.NodeB.Next;
}
contact.Destroy();
--ContactCount;
}
internal void Collide()
{
// Update awake contacts.
Contact c = ContactList;
while (c != null)
{
Fixture fixtureA = c.FixtureA;
Fixture fixtureB = c.FixtureB;
int indexA = c.ChildIndexA;
int indexB = c.ChildIndexB;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
if (bodyA.Awake == false && bodyB.Awake == false)
{
c = c.Next;
continue;
}
// Is this contact flagged for filtering?
if ((c.Flags & ContactFlags.Filter) == ContactFlags.Filter)
{
// Should these bodies collide?
if (bodyB.ShouldCollide(bodyA) == false)
{
Contact cNuke = c;
c = cNuke.Next;
Destroy(cNuke);
continue;
}
// Check default filtering
if (ShouldCollide(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
c = cNuke.Next;
Destroy(cNuke);
continue;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
Contact cNuke = c;
c = cNuke.Next;
Destroy(cNuke);
continue;
}
// Clear the filtering flag.
c.Flags &= ~ContactFlags.Filter;
}
int proxyIdA = fixtureA.Proxies[indexA].ProxyId;
int proxyIdB = fixtureB.Proxies[indexB].ProxyId;
bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB);
// Here we destroy contacts that cease to overlap in the broad-phase.
if (overlap == false)
{
Contact cNuke = c;
c = cNuke.Next;
Destroy(cNuke);
continue;
}
// The contact persists.
c.Update(this);
c = c.Next;
}
}
// Broad-phase callback.
private void AddPair(ref FixtureProxy proxyA, ref FixtureProxy proxyB)
{
Fixture fixtureA = proxyA.Fixture;
Fixture fixtureB = proxyB.Fixture;
int indexA = proxyA.ChildIndex;
int indexB = proxyB.ChildIndex;
Body bodyA = fixtureA.Body;
Body bodyB = fixtureB.Body;
// Are the fixtures on the same body?
if (bodyA == bodyB)
{
return;
}
// Does a contact already exist?
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.Other == bodyA)
{
Fixture fA = edge.Contact.FixtureA;
Fixture fB = edge.Contact.FixtureB;
int iA = edge.Contact.ChildIndexA;
int iB = edge.Contact.ChildIndexB;
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 default filter
if (ShouldCollide(fixtureA, fixtureB) == false)
{
return;
}
// Check user filtering.
if (ContactFilter != null && ContactFilter(fixtureA, fixtureB) == false)
{
return;
}
if (fixtureA.BeforeCollision != null && fixtureA.BeforeCollision(fixtureA, fixtureB) == false)
{
return;
}
if (fixtureB.BeforeCollision != null && fixtureB.BeforeCollision(fixtureB, fixtureA) == false)
{
return;
}
// Call the factory.
Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB);
// Contact creation may swap fixtures.
fixtureA = c.FixtureA;
fixtureB = c.FixtureB;
indexA = c.ChildIndexA;
indexB = c.ChildIndexB;
bodyA = fixtureA.Body;
bodyB = fixtureB.Body;
// Insert into the world.
c.Prev = null;
c.Next = ContactList;
if (ContactList != null)
{
ContactList.Prev = c;
}
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;
++ContactCount;
}
public void Add(Body body)
{
Debug.Assert(this.BodyCount < this._bodyCapacity);
this.Bodies[this.BodyCount++] = body;
}
internal void SolveTOI(ref TimeStep subStep)
{
this._contactSolver.Reset(this._contacts, this.ContactCount, subStep.dtRatio, false);
// Solve position constraints.
const float kTOIBaumgarte = 0.75f;
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = this._contactSolver.SolvePositionConstraints(kTOIBaumgarte);
if (contactsOkay)
{
break;
}
if (i == Settings.TOIPositionIterations - 1)
{
i += 0;
}
}
// Leap of faith to new safe state.
for (int i = 0; i < this.BodyCount; ++i)
{
Body body = this.Bodies[i];
body.Sweep.A0 = body.Sweep.A;
body.Sweep.C0 = body.Sweep.C;
}
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
this._contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
{
this._contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
// Integrate positions.
for (int i = 0; i < this.BodyCount; ++i)
{
Body b = this.Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
// Check for large velocities.
float translationx = subStep.dt * b.LinearVelocityInternal.X;
float translationy = subStep.dt * b.LinearVelocityInternal.Y;
float dot = translationx * translationx + translationy * translationy;
if (dot > Settings.MaxTranslationSquared)
{
float norm = 1f / (float)Math.Sqrt(dot);
float value = Settings.MaxTranslation * subStep.inv_dt;
b.LinearVelocityInternal.X = value * (translationx * norm);
b.LinearVelocityInternal.Y = value * (translationy * norm);
}
float rotation = subStep.dt * b.AngularVelocity;
if (rotation * rotation > Settings.MaxRotationSquared)
{
if (rotation < 0.0)
{
b.AngularVelocityInternal = -subStep.inv_dt * Settings.MaxRotation;
}
else
{
b.AngularVelocityInternal = subStep.inv_dt * Settings.MaxRotation;
}
}
// Integrate
b.Sweep.C.X += subStep.dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += subStep.dt * b.LinearVelocityInternal.Y;
b.Sweep.A += subStep.dt * b.AngularVelocityInternal;
// Compute new transform
b.SynchronizeTransform();
// Note: shapes are synchronized later.
}
Report(this._contactSolver.Constraints);
}
/// <summary>
// Advance a dynamic body to its first time of contact
// and adjust the position to ensure clearance.
/// </summary>
/// <param name="body">The body.</param>
private 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.BodyType;
// Only bullets perform TOI with dynamic bodies.
if (bullet)
{
// 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.Enabled == false)
{
continue;
}
// Prevent infinite looping.
if (contact.TOICount > 10)
{
continue;
}
Fixture fixtureA = contact.FixtureA;
Fixture fixtureB = contact.FixtureB;
int indexA = contact.ChildIndexA;
int indexB = contact.ChildIndexB;
// 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.Shape, indexA);
input.ProxyB.Set(fixtureB.Shape, 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);
if (toiContact.Enabled == 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.MaxTOIContacts); ce = ce.Next)
{
Body other = ce.Other;
BodyType type = other.BodyType;
// 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.Enabled == 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);
}
// Did the user disable the contact?
if (contact.Enabled == 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);
const 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.BodyType != BodyType.Static)
{
toiContact.Flags |= ContactFlags.BulletHit;
}
}
private void ProcessRemovedJoints()
{
if (this._jointRemoveList.Count > 0)
{
foreach (Joint joint in this._jointRemoveList)
{
bool collideConnected = joint.CollideConnected;
// Remove from the world list.
this.JointList.Remove(joint);
// Disconnect from island graph.
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// Wake up connected bodies.
bodyA.Awake = true;
bodyB.Awake = true;
// Remove from body 1.
if (joint.EdgeA.Prev != null)
{
joint.EdgeA.Prev.Next = joint.EdgeA.Next;
}
if (joint.EdgeA.Next != null)
{
joint.EdgeA.Next.Prev = joint.EdgeA.Prev;
}
if (joint.EdgeA == bodyA.JointList)
{
bodyA.JointList = joint.EdgeA.Next;
}
joint.EdgeA.Prev = null;
joint.EdgeA.Next = null;
// Remove from body 2
if (joint.EdgeB.Prev != null)
{
joint.EdgeB.Prev.Next = joint.EdgeB.Next;
}
if (joint.EdgeB.Next != null)
{
joint.EdgeB.Next.Prev = joint.EdgeB.Prev;
}
if (joint.EdgeB == bodyB.JointList)
{
bodyB.JointList = joint.EdgeB.Next;
}
joint.EdgeB.Prev = null;
joint.EdgeB.Next = null;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge 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;
}
}
if (this.JointRemoved != null)
{
this.JointRemoved(joint);
}
}
this._jointRemoveList.Clear();
}
}
private void Solve(ref TimeStep step)
{
// Size the island for the worst case.
_island.Reset(BodyList.Count,
ContactManager.ContactCount,
JointList.Count,
ContactManager);
// Clear all the island flags.
foreach (Body b in BodyList)
{
b.Flags &= ~BodyFlags.Island;
}
for (Contact c = ContactManager.ContactList; c != null; c = c.Next)
{
c.Flags &= ~ContactFlags.Island;
}
foreach (Joint j in JointList)
{
j.IslandFlag = false;
}
// Build and simulate all awake islands.
int stackSize = BodyList.Count;
if (stackSize > _stack.Length)
{
_stack = new Body[Math.Max(_stack.Length * 2, stackSize)];
}
for (int index = BodyList.Count - 1; index >= 0; index--)
{
Body seed = BodyList[index];
if ((seed.Flags & (BodyFlags.Island)) != BodyFlags.None)
{
continue;
}
if (seed.Awake == false || seed.Active == false)
{
continue;
}
// The seed can be dynamic or kinematic.
if (seed.BodyType == 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 constraint graph.
while (stackCount > 0)
{
// Grab the next body off the stack and add it to the island.
Body b = _stack[--stackCount];
Debug.Assert(b.Active);
_island.Add(b);
// Make sure the body is awake.
b.Awake = true;
// To keep islands as small as possible, we don't
// propagate islands across static bodies.
if (b.BodyType == 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.Enabled || !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)
{
continue;
}
Body other = je.Other;
// WIP David
// Enter here when it's a non-fixed joint. Non-fixed joints have a other body.
if (other != null)
{
// Don't simulate joints connected to inactive bodies.
if (other.Active == 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;
}
else
{
_island.Add(je.Joint);
je.Joint.IslandFlag = true;
}
}
}
_island.Solve(ref step, Gravity);
// 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.BodyType == BodyType.Static)
{
b.Flags &= ~BodyFlags.Island;
}
}
}
// Synchronize fixtures, check for out of range bodies.
foreach (Body b in BodyList)
{
// If a body was not in an island then it did not move.
if ((b.Flags & BodyFlags.Island) != BodyFlags.Island)
{
continue;
}
if (b.BodyType == BodyType.Static)
{
continue;
}
// Update fixtures (for broad-phase).
b.SynchronizeFixtures();
}
// Look for new contacts.
ContactManager.FindNewContacts();
}
/// <summary>
/// Destroy a joint. This may cause the connected bodies to begin colliding.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="joint">The joint.</param>
public void RemoveJoint(Joint joint)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
bool collideConnected = joint.CollideConnected;
// Remove from the world list.
JointList.Remove(joint);
// Disconnect from island graph.
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// Wake up connected bodies.
bodyA.Awake = true;
// WIP David
if (!joint.IsFixedType())
{
bodyB.Awake = true;
}
// Remove from body 1.
if (joint.EdgeA.Prev != null)
{
joint.EdgeA.Prev.Next = joint.EdgeA.Next;
}
if (joint.EdgeA.Next != null)
{
joint.EdgeA.Next.Prev = joint.EdgeA.Prev;
}
if (joint.EdgeA == bodyA.JointList)
{
bodyA.JointList = joint.EdgeA.Next;
}
joint.EdgeA.Prev = null;
joint.EdgeA.Next = null;
// WIP David
if (!joint.IsFixedType())
{
// Remove from body 2
if (joint.EdgeB.Prev != null)
{
joint.EdgeB.Prev.Next = joint.EdgeB.Next;
}
if (joint.EdgeB.Next != null)
{
joint.EdgeB.Next.Prev = joint.EdgeB.Prev;
}
if (joint.EdgeB == bodyB.JointList)
{
bodyB.JointList = joint.EdgeB.Next;
}
joint.EdgeB.Prev = null;
joint.EdgeB.Next = null;
}
// WIP David
if (!joint.IsFixedType())
{
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge 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;
}
}
}
if (JointRemoved != null)
{
JointRemoved(joint);
}
}
/// <summary>
/// Create a joint to constrain bodies together. This may cause the connected bodies to cease colliding.
/// Warning: This function is locked during callbacks.
/// </summary>
/// <param name="joint">The joint.</param>
public void AddJoint(Joint joint)
{
Debug.Assert(!IsLocked);
if (IsLocked)
{
return;
}
// Connect to the world list.
JointList.Add(joint);
// Connect to the bodies' doubly linked lists.
joint.EdgeA.Joint = joint;
joint.EdgeA.Other = joint.BodyB;
joint.EdgeA.Prev = null;
joint.EdgeA.Next = joint.BodyA.JointList;
if (joint.BodyA.JointList != null)
{
joint.BodyA.JointList.Prev = joint.EdgeA;
}
joint.BodyA.JointList = joint.EdgeA;
// WIP David
if (!joint.IsFixedType())
{
joint.EdgeB.Joint = joint;
joint.EdgeB.Other = joint.BodyA;
joint.EdgeB.Prev = null;
joint.EdgeB.Next = joint.BodyB.JointList;
if (joint.BodyB.JointList != null)
{
joint.BodyB.JointList.Prev = joint.EdgeB;
}
joint.BodyB.JointList = joint.EdgeB;
}
// WIP David
if (!joint.IsFixedType())
{
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (joint.CollideConnected == false)
{
ContactEdge 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;
}
}
}
if (JointAdded != null)
{
JointAdded(joint);
}
// Note: creating a joint doesn't wake the bodies.
}
static public WeldJoint MakeWeldJoint(FBody a, FBody b, Frame3D bBodyOffset)
{
return(JointFactory.CreateWeldJoint(World, a, b, new Microsoft.Xna.Framework.Vector2(0, 0),
FarseerConverter.ToSimUnits(new Microsoft.Xna.Framework.Vector2((float)-bBodyOffset.X, (float)-bBodyOffset.Y))));
//без минусов не работает как надо
}
static public WeldJoint MakeWeldJoint(FBody a, FBody b)
{
//return JointFactory.CreateWeldJoint(World, a, b, new Microsoft.Xna.Framework.Vector2(0, 0));
return(MakeWeldJoint(a, b, new Frame3D(b.Position.X, b.Position.Y, 0)));
}