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 void createShip1(Vector2 pos, float _rot) { World world = Game1.GameInstance.getWorld(); //////////////// For Sprite System use /////////////// Sprite shipSprite = (Sprite)DisplayManager.Instance().getDisplayObj(SpriteEnum.Ship); Sprite_Proxy proxyShip = new Sprite_Proxy(shipSprite, pos.X, pos.Y, shipScale, Color.Blue); Ship p1 = new Ship(GameObjType.p1ship, proxyShip); SBNode shipBatch = SpriteBatchManager.Instance().getBatch(batchEnum.ships); shipBatch.addDisplayObject(proxyShip); ////////////////////////////////////// // Box2D Body Setup///////////////////////// var shipShape = new PolygonShape(); Vector2[] verts = new Vector2[5]; verts[0] = new Vector2(-5.0f, -5.0f); verts[1] = new Vector2(4.8f, -0.10f); verts[2] = new Vector2(5.0f, 0.00f); verts[3] = new Vector2(4.8f, 0.10f); verts[4] = new Vector2(-5.0f, 5.0f); shipShape.Set(verts, 5); shipShape._centroid = new Vector2(0, 0); var fd = new FixtureDef(); fd.shape = shipShape; fd.restitution = 0.9f; fd.friction = 0.0f; fd.density = 1.0f; fd.userData = p1; BodyDef bd = new BodyDef(); bd.allowSleep = false; bd.fixedRotation = true; bd.type = BodyType.Dynamic; bd.position = p1.spriteRef.pos; var body = world.CreateBody(bd); body.CreateFixture(fd); body.SetUserData(p1); body.Rotation = _rot; /////////////////////////////////////// // Set sprite body reference PhysicsMan.Instance().addPhysicsObj(p1, body); ////////////////// // Set Player's ship and add it to the GameObjManager ////////////// PlayerManager.Instance().getPlayer(PlayerID.one).setShip(p1); GameObjManager.Instance().addGameObj(p1); }
/// <summary> /// Creates a new ground component /// </summary> /// <param name="world">The Box2D world that will hold this component</param> /// <param name="content">Used ContentManager</param> /// <param name="pos">The position of this component</param> /// <param name="angle">The rotation angle of this component</param> /// <param name="width">The width of this component</param> /// <param name="height">The height of this component</param> public Ground(World world, ContentManager content, Vector2 pos, float angle, float width, float height) : base(world, content, pos, angle, width, height) { Name = "grass"; texture = content.Load<Texture2D>("Images/" + Name); PolygonShape shape = new PolygonShape(); shape.SetAsBox(width * 0.5f / Level.FACTOR, height * 0.5f / Level.FACTOR); body.CreateFixture(shape, 1.0f).SetFriction(1.0f); origin = new Vector2(screenPos.Width * 0.5f, texture.Height * 0.5f); sourceRect = new Rectangle(0, 0, (int)width, texture.Height); }
/// <summary> /// Implement Shape. /// </summary> /// <returns>Shape</returns> public override Shape Clone() { var clone = new PolygonShape(); clone.ShapeType = ShapeType; clone._radius = _radius; clone._vertexCount = _vertexCount; clone._centroid = _centroid; clone._vertices = _vertices; clone._normals = _normals; return clone; }
public LevelComponent(GameContent gameContent, World world) { this.gameContent = gameContent; this.world = world; levelData = gameContent.content.Load<LevelData>("Levels/level" + gameContent.levelIndex); MaxAtoms = levelData.MaxAtoms; int totalProbability = 0; for (int i = 0; i < levelData.AtomProbability.Length; i++) totalProbability += levelData.AtomProbability[i]; if (totalProbability != 100) throw new Exception("must be 100"); entryPoint = levelData.Entry; bonusType = levelData.BonusType; BodyDef bd = new BodyDef(); Body ground = world.CreateBody(bd); PolygonShape ps = new PolygonShape(); List<Vector2> v = levelData.ContinuousBoundry; for (int i = 0; i < v.Count - 1; i++) { if (v[i + 1].X == -1 && v[i + 1].Y == -1) { i++; continue; } ps.SetAsEdge(v[i] / gameContent.scale, v[i + 1] / gameContent.scale); ground.CreateFixture(ps, 0); } for (int i = 0; i < levelData.EquipmentDetails.Count; i++) { Equipment eq = new Equipment(levelData.EquipmentDetails[i].EquipmentName, gameContent, world); eq.body.Position = levelData.EquipmentDetails[i].Position / gameContent.scale; eq.body.Rotation = levelData.EquipmentDetails[i].RotationInDeg / 180 * (float)MathHelper.Pi; eq.isClamped = levelData.EquipmentDetails[i].IsClamped; eq.body.SetType(BodyType.Static); equipments.Add(eq); if (eq.equipName == EquipmentName.thermometer) thermometer = eq; if (eq.equipName == EquipmentName.pHscale) pHscale = eq; } }
/// <summary> /// Creates a new jump component /// </summary> /// <param name="world">The Box2D world that will hold this component</param> /// <param name="content">Used ContentManager</param> /// <param name="pos">The position of this component</param> /// <param name="angle">The rotation angle of this component</param> /// <param name="width">The width of this component</param> /// <param name="height">The height of this component</param> public Jump(World world, ContentManager content, Vector2 pos, float angle, float width, float height) : base(world, content, pos, angle, width, height) { Name = "jump"; texture = content.Load<Texture2D>("Images/" + Name); origin = new Vector2(texture.Width * 0.5f, texture.Height * 0.5f); sourceRect = new Rectangle(1, 1, texture.Width, texture.Height); PolygonShape shape = new PolygonShape(); Vector2[] vertices = {new Vector2(-width*0.5f/ Level.FACTOR, height*0.5f/ Level.FACTOR), new Vector2(width*0.5f/ Level.FACTOR, -height*0.5f/ Level.FACTOR), new Vector2(width*0.5f/ Level.FACTOR, height*0.5f/ Level.FACTOR)}; shape.Set(vertices, 3); body.CreateFixture(shape, 1.0f); }
public LabComponent(GameContent gameContent, World world) { this.gameContent = gameContent; EqScale = 0.4f; AtomScale = 0.7f; Width = (int)(gameContent.viewportSize.X * 2 / EqScale); Height = (int)(gameContent.viewportSize.Y * 1 / EqScale); gameContent.clampDistance = (int)(gameContent.viewportSize.X / 2 / EqScale); PolygonShape ps = new PolygonShape(); Body ground = world.CreateBody(new BodyDef()); Vector2 pos = new Vector2(Width / 2, Height) / gameContent.scale; ps.SetAsBox(Width / 2 / gameContent.scale, (float)gameContent.labTable.Height / gameContent.scale, pos, 0); ground.CreateFixture(ps, 0); }
public void addRect(float x, float y, float width, int height, object type) { PolygonShape ps = new PolygonShape(); ps.SetAsBox(width / PTM / 2, height / PTM / 2); FixtureDef fd = new FixtureDef(); fd.shape = ps; fd.restitution = 0.0f; fd.friction = 0.8f; fd.density = 1.0f; BodyDef bd = new BodyDef(); bd.type = BodyType.Static; bd.position = new Vector2(x / PTM, y / PTM); bd.userData = type; Body body = world.CreateBody(bd); body.CreateFixture(fd); }
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); }
public Body CreateWall(World world, float ScaleFactor) { var grounDef = new BodyDef(); grounDef.type = BodyType.Static; var groundFix = new FixtureDef(); groundFix.restitution = 1.0f; groundFix.friction = 0.0f; groundFix.density = 0.0f; var groundShape = new PolygonShape(); //groundShape.SetAsEdge(new Vector2(0, 8), new Vector2(4.8f, 8.0f)); groundShape.SetAsBox(texture.Width * ScaleFactor / 2f, texture.Height * ScaleFactor / 2f); var groundBody = world.CreateBody(grounDef); groundBody.Position = new Vector2(2.4f, 4); groundFix.shape = groundShape; groundBody.CreateFixture(groundFix); return groundBody; }
public PhysicsBox(Texture2D sprite_texture, World physicsWorld, float box_width, float box_height, float positionX, float positionY, float rot, float density) : base(sprite_texture) { this.scaleToFitTheseDimensions(box_width, box_height); this.position.X = positionX; this.position.Y = positionY; BodyDef dynamicBodyDef = new BodyDef(); dynamicBodyDef.type = BodyType.Dynamic; dynamicBodyDef.position = new Vector2(positionX/DynamicPhysicsGameObject.ScreenPixelsPerMeter, positionY/DynamicPhysicsGameObject.ScreenPixelsPerMeter); dynamicBodyDef.angle = rot; Body dynamicBody = physicsWorld.CreateBody(dynamicBodyDef); PolygonShape dynamicBoxShape = new PolygonShape(); dynamicBoxShape.SetAsBox((box_width/ScreenPixelsPerMeter) / 2.0f, (box_height/ScreenPixelsPerMeter) / 2.0f); //experiment with / 2.0f FixtureDef dynamicBoxFixtureDef = new FixtureDef(); dynamicBoxFixtureDef.shape = dynamicBoxShape; dynamicBoxFixtureDef.density = density; dynamicBoxFixtureDef.friction = 0.3f; dynamicBody.CreateFixture(dynamicBoxFixtureDef); this.physicsBody = dynamicBody; }
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); } }
void SetFixtures() { FixtureDef fd = new FixtureDef(); fd.density = 1; fd.friction = .3f; fd.restitution = 0f; PolygonShape ps = new PolygonShape(); float area; fixtureCount = 0; List<Vector2> v = equipmentData.ContinuousEdges; for (int i = 0; i < v.Count - 1; i++) { area = 0; if (v[i + 1].X == -1 && v[i + 1].Y == -1) { i++; continue; } Vector2 c = (v[i] + v[i + 1] - 2 * origin) / 2 / gameContent.b2Scale; float h = 8f / 2 / gameContent.b2Scale; float w = Vector2.Distance(v[i], v[i + 1]) / 2 / gameContent.b2Scale; float theta = (float)Math.Atan((v[i + 1].Y - v[i].Y) / (v[i + 1].X - v[i].X)); ps.SetAsBox(w, h, c, theta); fd.shape = ps; area = h * w * 4; fd.userData = area; fixtureCount += 1; body.CreateFixture(fd); } List<Box> b = equipmentData.Boxes; for (int i = 0; i < b.Count; i++) { float hx = b[i].Width / 2 / gameContent.b2Scale, hy = b[i].Height / 2 / gameContent.b2Scale; ps.SetAsBox(hx, hy, (b[i].Position - origin) / gameContent.b2Scale, b[i].RotationInDeg / 180 * (float)Math.PI); fd.shape = ps; area = hx * hy * 4; fd.userData = area; fixtureCount += 1; body.CreateFixture(fd); } List<Circle> circles = equipmentData.Circles; for (int i = 0; i < circles.Count; i++) { CircleShape cs = new CircleShape(); cs._radius = circles[i].Diameter / 2 / gameContent.b2Scale; cs._p = (circles[i].Position - origin) / gameContent.b2Scale; fd.shape = cs; area = (float)Math.PI * cs._radius * cs._radius; fd.userData = area; fixtureCount += 1; body.CreateFixture(fd); } }
/// <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); }
/// <summary> /// Creates a rectangle shaped part of the motorcycle or driver /// </summary> /// <param name="name">The Content name of the texture that belongs to this part</param> /// <param name="pos">The position of the part</param> /// <param name="width">The width of the rectangle shape</param> /// <param name="height">The height of the rectangle shape</param> /// <param name="angle">The rotation angle of the part</param> /// <param name="density">The density of the part</param> /// <param name="friction">The friction of the part</param> /// <param name="restitution">The restitution of the part</param> /// <returns></returns> private Body CreateBoxPart(String name, Vector2 pos, float width, float height, float angle, float density, float friction, float restitution) { PolygonShape shape = new PolygonShape(); shape.SetAsBox(width * 0.5f / Level.FACTOR, height * 0.5f / Level.FACTOR); Body body = CreatePart(shape, name, pos, angle, density, friction, restitution); ((UserData)body.GetUserData()).Width = width; ((UserData)body.GetUserData()).Height = height; return 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> /// Find the max separation between poly1 and poly2 using edge normals from poly1. /// </summary> /// <param name="edgeIndex"></param> /// <param name="poly1"></param> /// <param name="xf1"></param> /// <param name="poly2"></param> /// <param name="xf2"></param> /// <returns></returns> static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1, PolygonShape poly2, ref Transform xf2) { edgeIndex = -1; int count1 = poly1._vertexCount; // Vector pointing from the centroid of poly1 to the centroid of poly2. Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid); Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -Settings.b2_maxFloat; for (int i = 0; i < count1; ++i) { float dot = Vector2.Dot(poly1._normals[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2); // Find the best edge and the search direction. int bestEdge; float bestSeparation; int increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { edgeIndex = edge; return(s); } // Perform a local search for the best edge normal. for (; ;) { if (increment == -1) { edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; } else { edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; } s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } edgeIndex = bestEdge; return(bestSeparation); }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> /// <param name="poly1"></param> /// <param name="xf1"></param> /// <param name="edge1"></param> /// <param name="poly2"></param> /// <param name="xf2"></param> /// <returns></returns> static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) { int count1 = poly1._vertexCount; int count2 = poly2._vertexCount; Debug.Assert(0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. #if MATH_OVERLOADS Vector2 normal1World = MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]); Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, normal1World); #else Vector2 p1n = poly1._normals[edge1]; Vector2 normal1World = new Vector2(xf1.R.col1.X * p1n.X + xf1.R.col2.X * p1n.Y, xf1.R.col1.Y * p1n.X + xf1.R.col2.Y * p1n.Y); Vector2 normal1 = new Vector2(normal1World.X * xf2.R.col1.X + normal1World.Y * xf2.R.col1.Y, normal1World.X * xf2.R.col2.X + normal1World.Y * xf2.R.col2.Y); #endif // Find support vertex on poly2 for -normal. int index = 0; float minDot = Settings.b2_maxFloat; for (int i = 0; i < count2; ++i) { #if !MATH_OVERLOADS // inlining this made it 1ms slower float dot = Vector2.Dot(poly2._vertices[i], normal1); #else Vector2 p2vi = poly2._vertices[i]; float dot = p2vi.X * normal1.X + p2vi.Y * normal1.Y; #endif if (dot < minDot) { minDot = dot; index = i; } } #if MATH_OVERLOADS Vector2 v1 = MathUtils.Multiply(ref xf1, poly1._vertices[edge1]); Vector2 v2 = MathUtils.Multiply(ref xf2, poly2._vertices[index]); #else Vector2 p1ve = poly1._vertices[edge1]; Vector2 p2vi = poly2._vertices[index]; Vector2 v1 = new Vector2(xf1.Position.X + xf1.R.col1.X * p1ve.X + xf1.R.col2.X * p1ve.Y, xf1.Position.Y + xf1.R.col1.Y * p1ve.X + xf1.R.col2.Y * p1ve.Y); Vector2 v2 = new Vector2(xf2.Position.X + xf2.R.col1.X * p2vi.X + xf2.R.col2.X * p2vi.Y, xf2.Position.Y + xf2.R.col1.Y * p2vi.X + xf2.R.col2.Y * p2vi.Y); #endif #if !MATH_OVERLOADS // inlining is 1ms slower float separation = Vector2.Dot(v2 - v1, normal1World); #else Vector2 v2subv1 = new Vector2(v2.X - v1.X, v2.Y - v1.Y); float separation = v2subv1.X * normal1World.X + v2subv1.Y * normal1World.Y; #endif return separation; }
/// <summary> /// Compute the collision manifold between a polygon and a circle. /// </summary> /// <param name="manifold"></param> /// <param name="polygonA"></param> /// <param name="xfA"></param> /// <param name="circleB"></param> /// <param name="xfB"></param> public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold._pointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = MathUtils.Multiply(ref xfB, circleB._p); Vector2 cLocal = MathUtils.MultiplyT(ref xfA, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.b2_maxFloat; float radius = polygonA._radius + circleB._radius; int vertexCount = polygonA._vertexCount; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.Dot(polygonA._normals[i], cLocal - polygonA._vertices[i]); if (s > radius) { // Early out. return; } if (s > separation) { separation = s; normalIndex = i; } } // Vertices that subtend the incident face. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; Vector2 v1 = polygonA._vertices[vertIndex1]; Vector2 v2 = polygonA._vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.b2_epsilon) { manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localNormal = polygonA._normals[normalIndex]; manifold._localPoint = 0.5f * (v1 + v2); var p0 = manifold._points[0]; p0.LocalPoint = circleB._p; p0.Id.Key = 0; manifold._points[0] = p0; return; } // Compute barycentric coordinates float u1 = Vector2.Dot(cLocal - v1, v2 - v1); float u2 = Vector2.Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localNormal = cLocal - v1; manifold._localNormal.Normalize(); manifold._localPoint = v1; var p0b = manifold._points[0]; p0b.LocalPoint = circleB._p; p0b.Id.Key = 0; manifold._points[0] = p0b; } else if (u2 <= 0.0f) { if (Vector2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localNormal = cLocal - v2; manifold._localNormal.Normalize(); manifold._localPoint = v2; var p0c = manifold._points[0]; p0c.LocalPoint = circleB._p; p0c.Id.Key = 0; manifold._points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); float separation2 = Vector2.Dot(cLocal - faceCenter, polygonA._normals[vertIndex1]); if (separation2 > radius) { return; } manifold._pointCount = 1; manifold._type = ManifoldType.FaceA; manifold._localNormal = polygonA._normals[vertIndex1]; manifold._localPoint = faceCenter; var p0d = manifold._points[0]; p0d.LocalPoint = circleB._p; p0d.Id.Key = 0; manifold._points[0] = p0d; } }
static EPAxis ComputePolygonSeperation(Vector2 v1, Vector2 v2, Vector2 n, PolygonShape polygonB, float radius) { // PolygonB separation EPAxis axis; axis.type = EPAxisType.EdgeB; axis.index = 0; axis.separation = float.MinValue; for (int i = 0; i < polygonB._vertexCount; ++i) { float s1 = Vector2.Dot(polygonB._normals[i], v1 - polygonB._vertices[i]); float s2 = Vector2.Dot(polygonB._normals[i], v2 - polygonB._vertices[i]); float s = Math.Min(s1, s2); if (s > axis.separation) { axis.index = i; axis.separation = s; if (s > radius) { return axis; } } } return axis; }
static EPAxis ComputeEdgeSeperation(Vector2 v1, Vector2 v2, Vector2 n, PolygonShape polygonB, float radius) { // EdgeA separation EPAxis axis; axis.type = EPAxisType.EdgeA; axis.index = 0; axis.separation = Vector2.Dot(n, polygonB._vertices[0] - v1); for (int i = 1; i < polygonB._vertexCount; ++i) { float s = Vector2.Dot(n, polygonB._vertices[i] - v1); if (s < axis.separation) { axis.separation = s; } } return axis; }
public override void Initialize(ConfigSection section) { base.Initialize(section); textureName = section["texture"]; var pos = section["position"].AsVector2(); SideLength = section["sideLength"]; Speed = section["speed"]; MaxSpeed = section["maxSpeed"]; JumpImpulse = section["jumpImpulse"]; JumpThreshold = section["jumpThreshold"]; var bodyDef = new BodyDef(); bodyDef.type = BodyType.Dynamic; bodyDef.angle = 0; bodyDef.position = Game.level.ConvertToBox2D(pos); bodyDef.inertiaScale = section["inertiaScale"]; bodyDef.linearDamping = section["linearDamping"]; bodyDef.angularDamping = section["angularDamping"]; bodyDef.userData = this; Body = Game.level.World.CreateBody(bodyDef); var shape = new PolygonShape(); var offset = SideLength / 2; shape.Set(new Vector2[]{ Game.level.ConvertToBox2D(new Vector2(-offset, -offset)), Game.level.ConvertToBox2D(new Vector2(offset, -offset)), Game.level.ConvertToBox2D(new Vector2(offset, offset)), Game.level.ConvertToBox2D(new Vector2(-offset, offset)) } , 4); var fixture = new FixtureDef(); fixture.restitution = section["restitution"]; fixture.density = section["density"]; fixture.shape = shape; fixture.friction = section["friction"]; Body.CreateFixture(fixture); }
public override void Initialize(ConfigSection section) { base.Initialize(section); if (section.Options.ContainsKey("texture")) { _textureName = section["texture"]; } var pos = section["position"].AsVector2(); _dim = section["dimensions"].AsVector2(); if (section.Options.ContainsKey("enterEvent")) { _enterEvent = section["enterEvent"]; } if (section.Options.ContainsKey("leaveEvent")) { _leaveEvent = section["leaveEvent"]; } if (section.Options.ContainsKey("enterEventData")) { _enterEventData = section["enterEventData"]; } if (section.Options.ContainsKey("leaveEventData")) { _leaveEventData = section["leaveEventData"]; } var bodyDef = new BodyDef(); bodyDef.userData = this; var fixtureDef = new FixtureDef(); var shape = new PolygonShape(); shape.SetAsBox(Game.level.ConvertToBox2D(_dim.X / 2), Game.level.ConvertToBox2D(_dim.Y / 2)); fixtureDef.shape = shape; fixtureDef.isSensor = true; fixtureDef.userData = new LevelElementInfo() { type = LevelElementType.Ground }; bodyDef.position = Game.level.ConvertToBox2D(pos); Body = Game.level.World.CreateBody(bodyDef); Body.CreateFixture(fixtureDef); }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold"></param> /// <param name="polyA"></param> /// <param name="xfA"></param> /// <param name="polyB"></param> /// <param name="xfB"></param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB) { manifold._pointCount = 0; float totalRadius = polyA._radius + polyB._radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > totalRadius) { return; } int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > totalRadius) { return; } PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge byte flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; manifold._type = ManifoldType.FaceB; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold._type = ManifoldType.FaceA; flip = 0; } FixedArray2 <ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1._vertexCount; Vector2 v11 = poly1._vertices[edge1]; Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0]; Vector2 localTangent = v12 - v11; localTangent.Normalize(); Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent); Vector2 normal = MathUtils.Cross(tangent, 1.0f); v11 = MathUtils.Multiply(ref xf1, v11); v12 = MathUtils.Multiply(ref xf1, v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2 <ClipVertex> clipPoints1; FixedArray2 <ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1); if (np < 2) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold._localNormal = localNormal; manifold._localPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold._points[pointCount]; cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v); cp.Id = clipPoints2[i].id; cp.Id.Features.Flip = flip; manifold._points[pointCount] = cp; ++pointCount; } } manifold._pointCount = pointCount; }
protected void AddPoly(Body body2Body, V2DShape polygon) { Shape shape; if (polygon.IsCircle) { CircleShape circDef = new CircleShape(); circDef._radius = polygon.Radius / (V2DScreen.WorldScale * State.Scale.X); Vector2 lp = new Vector2(polygon.CenterX / V2DScreen.WorldScale, polygon.CenterY / V2DScreen.WorldScale); circDef._p = lp; shape = circDef; } else { float[] pts = polygon.Data; PolygonShape polyDef = new PolygonShape(); shape = polyDef; int len= (int)(pts.Length / 2); Vector2[] v2s = new Vector2[len]; for (int i = 0; i < len; i++) { float px = pts[i * 2]; float py = pts[i * 2 + 1]; v2s[i] = new Vector2( px / V2DScreen.WorldScale * State.Scale.X, py / V2DScreen.WorldScale * State.Scale.Y); } polyDef.Set(v2s, len); } FixtureDef fd = new FixtureDef(); fd.shape = shape; if (instanceName.IndexOf("s_") == 0) { isStatic = true; fd.density = 0.0f; } else { fd.density = density; } fd.friction = friction; fd.restitution = restitution; if (groupIndex != 0) { fd.filter.groupIndex = groupIndex; } if (attributeProperties != null) { attributeProperties.ApplyAttribtues(fd); } body.CreateFixture(fd); }
private void GenerateDrawVertexs() { Vector2[] vertexs = new Vector2[mCountBaseVertexs * 10]; for (int i = 1; i < mCountBaseVertexs - 2; i++) { for (int j = 0; j < 10; j++) { float x = (i - 1) * 10 * mScale + j * mScale; float y = CubicInterpolate( mTerrainBaseVertexs[i - 1], mTerrainBaseVertexs[i], mTerrainBaseVertexs[i + 1], mTerrainBaseVertexs[i + 2], j / 10f); vertexs[i * 10 + j] = new Vector2(x, y); } } int count = (mCountBaseVertexs - 3) * 10; mTerrainDrawVertexs = new Vector2[count + 3]; Array.Copy(vertexs, 10, mTerrainDrawVertexs, 0, count); for (int i = 0; i < count; i++) { mTerrainDrawVertexs[i].X -= ((mCountBaseVertexs - 4) * 10 * mScale + 9 * mScale) / 2f; mTerrainDrawVertexs[i].Y -= 50; } float y1 = -200; mTerrainDrawVertexs[count] = new Vector2(mTerrainDrawVertexs[count - 1].X, y1); mTerrainDrawVertexs[count + 1] = new Vector2(mTerrainDrawVertexs[0].X, y1); mTerrainDrawVertexs[count + 2] = mTerrainDrawVertexs[0]; for (int i = 0; i < count - 1; i++) { PolygonShape shape = new PolygonShape(); shape.SetAsEdge(mTerrainDrawVertexs[i], mTerrainDrawVertexs[i + 1]); mBody.CreateFixture(shape, 0.0f); } }
/// <summary> /// Find the max separation between poly1 and poly2 using edge normals from poly1. /// </summary> /// <param name="edgeIndex"></param> /// <param name="poly1"></param> /// <param name="xf1"></param> /// <param name="poly2"></param> /// <param name="xf2"></param> /// <returns></returns> static float FindMaxSeparation(out int edgeIndex, PolygonShape poly1, ref Transform xf1, PolygonShape poly2, ref Transform xf2) { edgeIndex = -1; int count1 = poly1._vertexCount; // Vector pointing from the centroid of poly1 to the centroid of poly2. Vector2 d = MathUtils.Multiply(ref xf2, poly2._centroid) - MathUtils.Multiply(ref xf1, poly1._centroid); Vector2 dLocal1 = MathUtils.MultiplyT(ref xf1.R, d); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -Settings.b2_maxFloat; for (int i = 0; i < count1; ++i) { float dot = Vector2.Dot(poly1._normals[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2); // Find the best edge and the search direction. int bestEdge; float bestSeparation; int increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { edgeIndex = edge; return s; } // Perform a local search for the best edge normal. for (; ; ) { if (increment == -1) edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; else edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } edgeIndex = bestEdge; return bestSeparation; }
static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, PolygonShape poly1, int edge1, PolygonShape poly2) { int count1 = poly1._vertexCount; int count2 = poly2._vertexCount; Debug.Assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vector2 normal1 = poly1._normals[edge1]; // Find the incident edge on poly2. int index = 0; float minDot = float.MaxValue; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(normal1, poly2._normals[i]); if (dot < minDot) { minDot = dot; index = i; } } // Build the clip vertices for the incident edge. int i1 = index; int i2 = i1 + 1 < count2 ? i1 + 1 : 0; ClipVertex ctemp = new ClipVertex(); ctemp.v = poly2._vertices[i1]; ctemp.id.Features.indexA = (byte)edge1; ctemp.id.Features.indexB = (byte)i1; ctemp.id.Features.typeA = (byte)ContactFeatureType.Face; ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex; c[0] = ctemp; ctemp.v = poly2._vertices[i2]; ctemp.id.Features.indexA = (byte)edge1; ctemp.id.Features.indexB = (byte)i2; ctemp.id.Features.typeA = (byte)ContactFeatureType.Face; ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex; c[1] = ctemp; }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold"></param> /// <param name="polyA"></param> /// <param name="xfA"></param> /// <param name="polyB"></param> /// <param name="xfB"></param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform xfA, PolygonShape polyB, ref Transform xfB) { manifold._pointCount = 0; float totalRadius = polyA._radius + polyB._radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > totalRadius) return; int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge byte flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; manifold._type = ManifoldType.FaceB; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold._type = ManifoldType.FaceA; flip = 0; } FixedArray2<ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1._vertexCount; Vector2 v11 = poly1._vertices[edge1]; Vector2 v12 = edge1 + 1 < count1 ? poly1._vertices[edge1 + 1] : poly1._vertices[0]; Vector2 localTangent = v12 - v11; localTangent.Normalize(); Vector2 localNormal = MathUtils.Cross(localTangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = MathUtils.Multiply(ref xf1.R, localTangent); Vector2 normal = MathUtils.Cross(tangent, 1.0f); v11 = MathUtils.Multiply(ref xf1, v11); v12 = MathUtils.Multiply(ref xf1, v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1); if (np < 2) return; // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold._localNormal = localNormal; manifold._localPoint = planePoint; int pointCount = 0; for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i) { float separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold._points[pointCount]; cp.LocalPoint = MathUtils.MultiplyT(ref xf2, clipPoints2[i].v); cp.Id = clipPoints2[i].id; cp.Id.Features.Flip = flip; manifold._points[pointCount] = cp; ++pointCount; } } manifold._pointCount = pointCount; }
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB_in, ref Transform xfB) { manifold._pointCount = 0; Transform xf; MathUtils.MultiplyT(ref xfA, ref xfB, out xf); // Create a polygon for edge shape A s_polygonA.SetAsEdge(edgeA._vertex1, edgeA._vertex2); // Build polygonB in frame A s_polygonB._radius = polygonB_in._radius; s_polygonB._vertexCount = polygonB_in._vertexCount; s_polygonB._centroid = MathUtils.Multiply(ref xf, polygonB_in._centroid); for (int i = 0; i < s_polygonB._vertexCount; ++i) { s_polygonB._vertices[i] = MathUtils.Multiply(ref xf, polygonB_in._vertices[i]); s_polygonB._normals[i] = MathUtils.Multiply(ref xf.R, polygonB_in._normals[i]); } float totalRadius = s_polygonA._radius + s_polygonB._radius; // Edge geometry Vector2 v1 = edgeA._vertex1; Vector2 v2 = edgeA._vertex2; Vector2 e = v2 - v1; Vector2 edgeNormal = new Vector2(e.Y, -e.X); edgeNormal.Normalize(); // Determine side bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB._centroid - v1) >= 0.0f; if (isFrontSide == false) { edgeNormal = -edgeNormal; } // Compute primary separating axis EPAxis edgeAxis = ComputeEdgeSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius); if (edgeAxis.separation > totalRadius) { // Shapes are separated return; } // Classify adjacent edges FixedArray2<EdgeType> types = new FixedArray2<EdgeType>(); //types[0] = EdgeType.Isolated; //types[1] = EdgeType.Isolated; if (edgeA._hasVertex0) { Vector2 v0 = edgeA._vertex0; float s = Vector2.Dot(edgeNormal, v0 - v1); if (s > 0.1f * Settings.b2_linearSlop) { types[0] = EdgeType.Concave; } else if (s >= -0.1f * Settings.b2_linearSlop) { types[0] = EdgeType.Flat; } else { types[0] = EdgeType.Convex; } } if (edgeA._hasVertex3) { Vector2 v3 = edgeA._vertex3; float s = Vector2.Dot(edgeNormal, v3 - v2); if (s > 0.1f * Settings.b2_linearSlop) { types[1] = EdgeType.Concave; } else if (s >= -0.1f * Settings.b2_linearSlop) { types[1] = EdgeType.Flat; } else { types[1] = EdgeType.Convex; } } if (types[0] == EdgeType.Convex) { // Check separation on previous edge. Vector2 v0 = edgeA._vertex0; Vector2 e0 = v1 - v0; Vector2 n0 = new Vector2(e0.Y, -e0.X); n0.Normalize(); if (isFrontSide == false) { n0 = -n0; } EPAxis axis1 = ComputeEdgeSeperation(v0, v1, n0, s_polygonB, totalRadius); if (axis1.separation > edgeAxis.separation) { // The polygon should collide with previous edge return; } } if (types[1] == EdgeType.Convex) { // Check separation on next edge. Vector2 v3 = edgeA._vertex3; Vector2 e2 = v3 - v2; Vector2 n2 = new Vector2(e2.Y, -e2.X); n2.Normalize(); if (isFrontSide == false) { n2 = -n2; } EPAxis axis2 = ComputeEdgeSeperation(v2, v3, n2, s_polygonB, totalRadius); if (axis2.separation > edgeAxis.separation) { // The polygon should collide with the next edge return; } } EPAxis polygonAxis = ComputePolygonSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius); if (polygonAxis.separation > totalRadius) { return; } // Use hysteresis for jitter reduction. float k_relativeTol = 0.98f; float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } PolygonShape poly1; PolygonShape poly2; if (primaryAxis.type == EPAxisType.EdgeA) { poly1 = s_polygonA; poly2 = s_polygonB; if (isFrontSide == false) { primaryAxis.index = 1; } manifold._type = ManifoldType.FaceA; } else { poly1 = s_polygonB; poly2 = s_polygonA; manifold._type = ManifoldType.FaceB; } int edge1 = primaryAxis.index; FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>(); FindIncidentEdge(ref incidentEdge, poly1, primaryAxis.index, poly2); int count1 = poly1._vertexCount; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = poly1._vertices[iv1]; Vector2 v12 = poly1._vertices[iv2]; Vector2 tangent = v12 - v11; tangent.Normalize(); Vector2 normal = MathUtils.Cross(tangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + totalRadius; float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < Settings.b2_maxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < Settings.b2_maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == EPAxisType.EdgeA) { manifold._localNormal = normal; manifold._localPoint = planePoint; } else { manifold._localNormal = MathUtils.MultiplyT(ref xf.R, normal); manifold._localPoint = MathUtils.MultiplyT(ref xf, planePoint); } int pointCount = 0; for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i) { float separation; separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold._points[pointCount]; if (primaryAxis.type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MultiplyT(ref xf, clipPoints2[i].v); cp.Id = clipPoints2[i].id; } else { cp.LocalPoint = clipPoints2[i].v; cp.Id.Features.typeA = clipPoints2[i].id.Features.typeB; cp.Id.Features.typeB = clipPoints2[i].id.Features.typeA; cp.Id.Features.indexA = clipPoints2[i].id.Features.indexB; cp.Id.Features.indexB = clipPoints2[i].id.Features.indexA; } manifold._points[pointCount] = cp; if (cp.Id.Features.typeA == (byte)ContactFeatureType.Vertex && types[cp.Id.Features.indexA] == EdgeType.Flat) { continue; } ++pointCount; } } manifold._pointCount = pointCount; }
static void FindIncidentEdge(out FixedArray2<ClipVertex> c, PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2) { c = new FixedArray2<ClipVertex>(); int count1 = poly1._vertexCount; int count2 = poly2._vertexCount; Debug.Assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1])); // Find the incident edge on poly2. int index = 0; float minDot = Settings.b2_maxFloat; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(normal1, poly2._normals[i]); if (dot < minDot) { minDot = dot; index = i; } } // Build the clip vertices for the incident edge. int i1 = index; int i2 = i1 + 1 < count2 ? i1 + 1 : 0; var cv0 = c[0]; cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]); cv0.id.Features.ReferenceEdge = (byte)edge1; cv0.id.Features.IncidentEdge = (byte)i1; cv0.id.Features.IncidentVertex = 0; c[0] = cv0; var cv1 = c[1]; cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]); cv1.id.Features.ReferenceEdge = (byte)edge1; cv1.id.Features.IncidentEdge = (byte)i2; cv1.id.Features.IncidentVertex = 1; c[1] = cv1; }