使用 Box2D 做一個 JansenWalker 機器人

在 Box2DFlash 的官網的首頁有一個小 Demo，這個 Demo 中有11個例子，可以通過左右方向鍵查看不同的例子，里面的每個例子都非常有趣，但最讓我感興趣的，是其中一個叫 JansenWalker 的，里面是一個往右移動的機器人，有6只腳，交替着地往右邊行走，如下圖：

前段時間在看 Box2D，把官網下載下來的 Demo 源碼都看完並寫一遍，其他的例子花的時間都不多，這個花的時間有點長，主要是分析結構，然后就是各個關節的比例，身體的大小，不斷的微調，找到合適的數值，以下是我最終完成的效果：

我加了4條腿，當然，理論上是可以N條腿的，腿越多越穩定，理論上是這樣，不過實際結果如何有興趣的童鞋可以自己試試，接下來我把我的制作過程記錄下來。

預備知識

完成這個 Demo 需要用到一些 Box2D 的特性，以下，是你需要知道的東西：

1、創建圓形剛體
2、創建矩形剛體
3、創建多邊形剛體
4、創建距離關節
5、創建旋轉關節
6、二維矩陣變換
7、碰撞檢測過濾

因為每次創建剛體都要寫很多代碼，所以我包裝了一部分創建剛體和關節的代碼，所有的代碼都實現在 b2Utils 這個類里面，以下是代碼：

package org.easily.box2d 
{
    import Box2D.Collision.Shapes.b2CircleShape;
    import Box2D.Collision.Shapes.b2PolygonShape;
    import Box2D.Common.Math.b2Vec2;
    import Box2D.Dynamics.Contacts.b2Contact;
    import Box2D.Dynamics.Contacts.b2ContactEdge;
    import Box2D.Dynamics.Joints.b2DistanceJoint;
    import Box2D.Dynamics.Joints.b2DistanceJointDef;
    import Box2D.Dynamics.Joints.b2MouseJoint;
    import Box2D.Dynamics.Joints.b2MouseJointDef;
    import Box2D.Dynamics.Joints.b2RevoluteJoint;
    import Box2D.Dynamics.Joints.b2RevoluteJointDef;
    import Box2D.Dynamics.b2Body;
    import Box2D.Dynamics.b2BodyDef;
    import Box2D.Dynamics.b2DebugDraw;
    import Box2D.Dynamics.b2Fixture;
    import Box2D.Dynamics.b2FixtureDef;
    import Box2D.Dynamics.b2World;
    
    import flash.display.Sprite;
    import flash.display.Stage;
    import flash.events.Event;
    import flash.events.MouseEvent;
    
    /**
     * box2d utils
     * @author Easily
     */
    public class b2Utils 
    {
        public static const frameRate:Number = 30;
        public static const timeStep:Number = 1.0 / frameRate;
        public static const velIterations:int = 10;
        public static const posIterations:int = 10;
        public static const worldScale:Number = 30;
        
        private static function convertVec2(vec2:b2Vec2):b2Vec2
        {
            vec2 = vec2.Copy();
            vec2.x /= worldScale;
            vec2.y /= worldScale;
            return vec2;
        }

        public static function createRect(world:b2World, type:uint, pos:b2Vec2, size:b2Vec2, density:Number, friction:Number, restitution:Number, filterIndex:int = 0):b2Body
        {
            pos = convertVec2(pos);
            size = convertVec2(size);
            
            var bodyDef:b2BodyDef = new b2BodyDef();
            bodyDef.position.Set(pos.x, pos.y);
            bodyDef.type = type;
            
            var shapeDef:b2PolygonShape = new b2PolygonShape();
            shapeDef.SetAsBox(size.x, size.y);
            
            var fixtureDef:b2FixtureDef = new b2FixtureDef();
            fixtureDef.density = density;
            fixtureDef.friction = friction;
            fixtureDef.restitution = restitution;
            fixtureDef.shape = shapeDef;
            fixtureDef.filter.groupIndex = filterIndex;
            
            var body:b2Body = world.CreateBody(bodyDef);
            body.CreateFixture(fixtureDef);
            return body;
        }

        public static function createRect2(world:b2World, type:uint, pos:b2Vec2, size:b2Vec2, offset:b2Vec2, angle:Number, density:Number, friction:Number, restitution:Number, filterIndex:int = 0):b2Body
        {
            pos = convertVec2(pos);
            size = convertVec2(size);
            offset = convertVec2(offset);
            
            var bodyDef:b2BodyDef = new b2BodyDef();
            bodyDef.position.Set(pos.x, pos.y);
            bodyDef.type = type;
            
            var shapeDef:b2PolygonShape = new b2PolygonShape();
            shapeDef.SetAsOrientedBox(size.x, size.y, offset, Math.PI / 180 * angle);
            
            var fixtureDef:b2FixtureDef = new b2FixtureDef();
            fixtureDef.density = density;
            fixtureDef.friction = friction;
            fixtureDef.restitution = restitution;
            fixtureDef.shape = shapeDef;
            fixtureDef.filter.groupIndex = filterIndex;
            
            var body:b2Body = world.CreateBody(bodyDef);
            body.CreateFixture(fixtureDef);
            return body;
        }

        public static function createPolygon(world:b2World, type:uint, pos:b2Vec2, vertices:Object, density:Number, friction:Number, restitution:Number, filterIndex:int = 0):b2Body
        {
            pos = convertVec2(pos);
            var vertices_:Array = [];
            for each (var vertex:b2Vec2 in vertices)
            {
                vertices_.push(convertVec2(vertex));
            }
            
            var bodyDef:b2BodyDef = new b2BodyDef();
            bodyDef.position.Set(pos.x, pos.y);
            bodyDef.type = type;
            
            var shapeDef:b2PolygonShape = new b2PolygonShape();
            shapeDef.SetAsArray(vertices_, vertices_.length);
            
            var fixtureDef:b2FixtureDef = new b2FixtureDef();
            fixtureDef.density = density;
            fixtureDef.friction = friction;
            fixtureDef.restitution = restitution;
            fixtureDef.shape = shapeDef;
            fixtureDef.filter.groupIndex = filterIndex;
            
            var body:b2Body = world.CreateBody(bodyDef);
            body.CreateFixture(fixtureDef);
            return body;
        }

        public static function createCircle(world:b2World, type:uint, pos:b2Vec2, radius:Number, density:Number, friction:Number, restitution:Number, filterIndex:int = 0):b2Body
        {
            pos = convertVec2(pos);
            radius /= worldScale;
            
            var bodyDef:b2BodyDef = new b2BodyDef();
            bodyDef.position.Set(pos.x, pos.y);
            bodyDef.type = type;
            
            var shapeDef:b2CircleShape = new b2CircleShape(radius);
            
            var fixtureDef:b2FixtureDef = new b2FixtureDef();
            fixtureDef.density = density;
            fixtureDef.friction = friction;
            fixtureDef.restitution = restitution;
            fixtureDef.shape = shapeDef;
            fixtureDef.filter.groupIndex = filterIndex;
            
            var body:b2Body = world.CreateBody(bodyDef);
            body.CreateFixture(fixtureDef);
            return body;
        }

        public static function createWall(world:b2World, w:Number, h:Number, thickness:Number, density:Number, friction:Number, restitution:Number):void
        {
            var up:b2Body = createRect(world, b2Body.b2_staticBody, new b2Vec2(w / 2, 0), new b2Vec2(w / 2, thickness), density, friction, restitution);
            var bottom:b2Body = createRect(world, b2Body.b2_staticBody, new b2Vec2(w / 2, h), new b2Vec2(w / 2, thickness), density, friction, restitution);
            var left:b2Body = createRect(world, b2Body.b2_staticBody, new b2Vec2(0, h / 2), new b2Vec2(thickness, h / 2), density, friction, restitution);    
            var right:b2Body = createRect(world, b2Body.b2_staticBody, new b2Vec2(w, h / 2), new b2Vec2(thickness, h / 2), density, friction, restitution);    
        }

        public static function createDebug(host:Sprite, world:b2World, flags:uint, alpha:Number):void
        {
            var sprite:Sprite = new Sprite();
            host.addChild(sprite);
            
            var debug:b2DebugDraw = new b2DebugDraw();
            debug.SetSprite(sprite);
            debug.SetDrawScale(worldScale);
            debug.SetFlags(flags);
            debug.SetFillAlpha(alpha);
            world.SetDebugDraw(debug);
        }

        public static function mouseToWorld(stage:Stage):b2Vec2
        {
            return new b2Vec2(stage.mouseX / worldScale, stage.mouseY / worldScale);
        }

        public static function mouseJoint(stage:Stage, world:b2World):void
        {
            var mouseJoint:b2MouseJoint;
            stage.addEventListener(MouseEvent.MOUSE_DOWN, createJoint);
            function createJoint(event:MouseEvent):void
            {
                world.QueryPoint(queryPoint, mouseToWorld(stage));
            }
            function queryPoint(fixture:b2Fixture):Boolean
            {
                var body:b2Body = fixture.GetBody();
                if (body.GetType() == b2Body.b2_dynamicBody)
                {
                    var jointDef:b2MouseJointDef = new b2MouseJointDef();
                    jointDef.bodyA = world.GetGroundBody();
                    jointDef.bodyB = body;
                    jointDef.target = mouseToWorld(stage);
                    jointDef.maxForce = 1000 * body.GetMass();
                    mouseJoint = world.CreateJoint(jointDef) as b2MouseJoint;
                    stage.addEventListener(MouseEvent.MOUSE_MOVE, moveJoint);
                    stage.addEventListener(MouseEvent.MOUSE_UP, killJoint);    
                }
                return false;
            }
            function moveJoint(event:MouseEvent):void
            {
                mouseJoint.SetTarget(mouseToWorld(stage));
            }
            function killJoint(event:MouseEvent):void
            {
                world.DestroyJoint(mouseJoint);
                stage.removeEventListener(MouseEvent.MOUSE_MOVE, moveJoint);
                stage.removeEventListener(MouseEvent.MOUSE_UP, killJoint);
            }
        }

        public static function distanceJoint(world:b2World, bodyA:b2Body, bodyB:b2Body, localAnchorA:b2Vec2, localAnchorB:b2Vec2, length:Number):b2DistanceJoint
        {
            localAnchorA = convertVec2(localAnchorA);
            localAnchorB = convertVec2(localAnchorB);
            length /= worldScale;
                
            var jointDef:b2DistanceJointDef = new b2DistanceJointDef();
            jointDef.bodyA = bodyA;
            jointDef.bodyB = bodyB;
            jointDef.localAnchorA = localAnchorA;
            jointDef.localAnchorB = localAnchorB;
            jointDef.length = length;
            var joint:b2DistanceJoint = world.CreateJoint(jointDef) as b2DistanceJoint;
            return joint;
        }

        public static function revoluteJoint(world:b2World, bodyA:b2Body, bodyB:b2Body, localAnchorA:b2Vec2, localAnchorB:b2Vec2, enableMotor:Boolean = false, motorSpeed:Number = 0, maxMotorTorque:Number = 0):b2RevoluteJoint
        {
            localAnchorA = convertVec2(localAnchorA);
            localAnchorB = convertVec2(localAnchorB);
            
            var jointDef:b2RevoluteJointDef = new b2RevoluteJointDef();
            jointDef.bodyA = bodyA;
            jointDef.bodyB = bodyB;
            jointDef.localAnchorA = localAnchorA;
            jointDef.localAnchorB = localAnchorB;
            
            if (enableMotor)
            {
                jointDef.enableMotor = true;
                jointDef.motorSpeed = motorSpeed;
                jointDef.maxMotorTorque = maxMotorTorque;
            }
            
            var joint:b2RevoluteJoint = world.CreateJoint(jointDef) as b2RevoluteJoint;
            return joint;
        }
    }
}

簡單介紹一下里面的方法：

a、createRect2: 同上，但可以指定旋轉角度以及相對中心點的偏移
b、createPolygon: 創建多邊形，傳入一個點數組
c、createCircle: 創建圓形
d、createWall: 創建包圍盒，方便測試
e、createDebug: 創建調試繪圖
f、mouseJoint: 鼠標關節
g、distanceJoint: 距離關節
h、revoluteJoint: 旋轉關節

其中需要注意的是 createPolygon，參數中的點數組，點的順序必須是順時針，逆時針的時候碰撞檢測失效。

結構分析

一開始我盯着這個機器人盯了很久，因為每條腿的顏色都是一樣的，所以看起來眼睛有點花，不過最后還是搞明白了基本的結構。這個機器人由幾種基本的形狀組成的，其中的三角形具有穩定性，因而不發生形變，而兩個四邊形在中間的節點的帶動下發生形變，從而帶動腿的運動。我自己在紙上畫了一些草圖，不過都非常凌亂。后面我 Google 了一下，發現原來這玩意最開始是一個實體的機器人，利用風能驅動，找到一張 gif 可以比較直觀的看到它的結構：

從圖中可以看到，主要結構由身體加兩邊的腿組成，身體是由一個圓形和矩形物體組合成，圓形旋轉作為整個機器人的驅動源，腿由兩個三角形組成，一個機器人可能會有N條腿。

創建身體

身體由一個圓和矩形組成，然后在圓和矩形的中心點創建一個旋轉關節，並且馬達開關打開，馬達速度為浮點數，正負分別代表順時針和逆時針轉動，代碼如下：

var type:int = b2Body.b2_dynamicBody;
var density:Number = 2;
var friction:Number = 0.5;
var resititution:Number = 0.3;
var radius:Number = 28;
var size:b2Vec2 = new b2Vec2(45, radius / 2);

var rect:b2Body = b2Utils.createRect(world, type, new b2Vec2, size, density, friction, resititution, bodyIndex);
var circle:b2Body = b2Utils.createCircle(world, type, new b2Vec2, radius, density, friction, resititution, bodyIndex);
b2Utils.revoluteJoint(world, rect, circle, new b2Vec2, new b2Vec2, true, 3, 1000);

創建腿

腿由兩個三角形組成，左右兩邊的腿是水平翻轉的關系，所以先以右邊的腿為例。上面的三角形為一個等邊三角形，下面為銳角三角形，兩個三角形有一個邊平等且長度相同，並且在這條邊的兩上端分別有一個距離關節，用於固定和連接兩個三角形，代碼如下：

var legW:Number = 50;
var legH:Number = 75;
var legLen1:Number = 55;
var legLen2:Number = 55;
var legIndex:int = -2;

var v1:b2Vec2 = new b2Vec2;
var v2:b2Vec2 = new b2Vec2(0,-legW);
var v3:b2Vec2 = new b2Vec2(legW,0);

var v4:b2Vec2 = new b2Vec2;
var v5:b2Vec2 = new b2Vec2(legW,0);
var v6:b2Vec2 = new b2Vec2(0,legH);

//upper leg
var vertices1:Array = [v1, v2, v3];
var triangle1:b2Body = b2Utils.createPolygon(world, type, new b2Vec2, vertices1, density, friction, resititution, legIndex);
bodies.push(triangle1);

//lower leg
var vertices2:Array = [v4, v5, v6];
var triangle2:b2Body = b2Utils.createPolygon(world, type, new b2Vec2, vertices2, density, friction, resititution, legIndex);
bodies.push(triangle2);

//connect two legs
b2Utils.distanceJoint(world, triangle1, triangle2, new b2Vec2, new b2Vec2, legLen1);
b2Utils.distanceJoint(world, triangle1, triangle2, b2Math.MulMV(m, new b2Vec2(legW, 0)), b2Math.MulMV(m, new b2Vec2(legW, 0)), legLen2);

好了，右腿創建出來了，接下來的問題是，Box2D 可以水平翻轉剛體嗎？很遺憾，不能，沒有相關的 API 干這個事。由於創建多邊形點順序的問題，一定要順時針，所以並不能直接把點翻轉過來。所以，創建左邊的三角形需要改點東西，看代碼：

var m:b2Mat22 = new b2Mat22();
m.Set(Math.PI);
var vertices1:Array = [v1, b2Math.MulMV(m, v3), v2];
var vertices2:Array = [v4, v6, b2Math.MulMV(m, v5)];

首先，保證點的順序為順時針，然后，通過 b2Math.MulMV 進行矩陣轉換，將點旋轉180度，這樣創建出來的三角形就是左邊的了。

組裝

身體和腿都出來了，接下來就是把這兩部分組裝起來。首先，上面的三角形需要固定在身體的兩側，然后上面的三角形的上面的點利用距離關節接連到身體上的圓的邊緣，最后，下面的三角形中間的點接連到圓上相同的位置，必須有兩對以上的腿機器人才能站穩。

這里有一個問題，如果有2對腿，那連接到圓上的點應該是在圓的兩側，如果有3對腿，那每個點與圓心連線的夾角應該是 360 / 3，也就是 2 * PI / 3，如何找到圓上的這幾個點呢？我試過3種方法，都是可行的，我最終選擇了最簡單的第3種方法：

1、已知半徑和角度的情況下，用勾股定理算出來
2、找到第一個點，然后轉換為圓的局部坐標，接着圓旋轉 2 * PI / n，再將局部坐標轉換成全局坐標
3、找到第一個點，然后使用 b2Math.MulMV 轉換

整合測試

點的轉換的代碼沒給了，直接給出所有的代碼，可以邊調試邊看。

package testbox2d
{
    import Box2D.Common.Math.b2Vec2;
    import Box2D.Dynamics.b2DebugDraw;
    import Box2D.Dynamics.b2World;
    
    import flash.display.Sprite;
    import flash.events.Event;
    
    import org.easily.box2d.b2Utils;
    import org.easily.test.box2d.JansenWalker;
        
    public class TestJansenWalker extends Sprite
    {
        private var world:b2World;
        private var walker:JansenWalker;
        
        public function TestJansenWalker()
        {
            super();
            
            if (!stage)
            {
                addEventListener(Event.ADDED_TO_STAGE, createWorld);
            }
            else
            {
                createWorld(null);
            }
        }
        
        private function createWorld(event:Event=null):void
        {
            var sleep:Boolean = true;
            var gravity:b2Vec2 = new b2Vec2(0, 9.81);
            world = new b2World(gravity, sleep);
            
            createWalker();
            b2Utils.createWall(world, stage.stageWidth, stage.stageHeight, 5, 2, 0.3, 0.5);
            b2Utils.mouseJoint(stage, world);
            b2Utils.createDebug(this, world, b2DebugDraw.e_jointBit | b2DebugDraw.e_shapeBit, 0.5);
            
            addEventListener(Event.ENTER_FRAME, onUpdate);
        }
        
        private function createWalker():void
        {
            walker = new JansenWalker(world);
            walker.setPosition(new b2Vec2(150, 350));
        }
        
        private function onUpdate(event:Event):void
        {
            world.Step(b2Utils.timeStep, b2Utils.velIterations, b2Utils.posIterations);
            world.ClearForces();
            world.DrawDebugData();
        }
    }
}

package org.easily.test.box2d
{
    import Box2D.Common.Math.b2Mat22;
    import Box2D.Common.Math.b2Math;
    import Box2D.Common.Math.b2Vec2;
    import Box2D.Dynamics.b2Body;
    import Box2D.Dynamics.b2FilterData;
    import Box2D.Dynamics.b2World;
    
    import org.easily.box2d.b2Utils;
    
    /**
     * jansenWalker by box2d
     * @author Easily
     */
    public class JansenWalker
    {
        private var world:b2World;
        private var center:b2Body;
        private var bodies:Array = [];
        
        public function JansenWalker(world_:b2World)
        {
            world = world_;
            createWalker();
        }
        
        public function setPosition(pos:b2Vec2):void
        {
            pos = pos.Copy();
            pos.x /= b2Utils.worldScale;
            pos.y /= b2Utils.worldScale;
            
            var offset:b2Vec2 = center.GetPosition().Copy();
            offset.Subtract(pos);
            
            center.SetPosition(pos);
            for each (var body:b2Body in bodies)
            {
                var old:b2Vec2 = body.GetPosition().Copy();
                old.Subtract(offset);
                body.SetPosition(old);
            }
        }
        
        private function createWalker():void
        {
            var type:int = b2Body.b2_dynamicBody;
            var density:Number = 2;
            var friction:Number = 0.5;
            var resititution:Number = 0.3;
            
            var radius:Number = 28;
            var size:b2Vec2 = new b2Vec2(45, radius / 2);
            
            var legNum:Number = 4;
            
            var legW:Number = 50;
            var legH:Number = 75;
            var legLen1:Number = 55;
            var legLen2:Number = 55;
            
            var jointLen1:Number = 88;
            var jointLen2:Number = 85;
            
            var bodyIndex:int = -1;
            var legIndex:int = -2;
            
            //motor
            var rect:b2Body = b2Utils.createRect(world, type, new b2Vec2, size, density, friction, resititution, bodyIndex);
            var circle:b2Body = b2Utils.createCircle(world, type, new b2Vec2, radius, density, friction, resititution, bodyIndex);
            b2Utils.revoluteJoint(world, rect, circle, new b2Vec2, new b2Vec2, true, 3, 1000);
            center = circle;
            bodies.push(rect);
            
            //legs
            var average:Number = 2 * Math.PI / legNum;
            var anchor:b2Vec2, first:b2Vec2 = new b2Vec2(radius * 2 / 3, 0);
            var m:b2Mat22 = new b2Mat22();
            
            var leftm:b2Mat22 = new b2Mat22();
            var rightm:b2Mat22 = new b2Mat22();
            leftm.Set(Math.PI);
            
            for (var i:int = 0; i < legNum; i++)
            {
                m.Set(i * average);
                anchor = b2Math.MulMV(m, first);
                createLeg(false, leftm);
                createLeg(true, rightm);
            }
            
            function createLeg(right:Boolean, m:b2Mat22):void
            {
                var v1:b2Vec2 = new b2Vec2;
                var v2:b2Vec2 = new b2Vec2(0,-legW);
                var v3:b2Vec2 = new b2Vec2(legW,0);
                
                var v4:b2Vec2 = new b2Vec2;
                var v5:b2Vec2 = new b2Vec2(legW,0);
                var v6:b2Vec2 = new b2Vec2(0,legH);
                
                //upper leg
                var vertices1:Array = right ? [v1, v2, v3] : [v1, b2Math.MulMV(m, v3), v2];
                var triangle1:b2Body = b2Utils.createPolygon(world, type, new b2Vec2, vertices1, density, friction, resititution, legIndex);
                bodies.push(triangle1);
                
                //lower leg
                var vertices2:Array = right ? [v4, v5, v6] : [v4, v6, b2Math.MulMV(m, v5)];
                var triangle2:b2Body = b2Utils.createPolygon(world, type, new b2Vec2, vertices2, density, friction, resititution, legIndex);
                bodies.push(triangle2);
                
                //connect two legs
                b2Utils.distanceJoint(world, triangle1, triangle2, new b2Vec2, new b2Vec2, legLen1);
                b2Utils.distanceJoint(world, triangle1, triangle2, b2Math.MulMV(m, new b2Vec2(legW, 0)), b2Math.MulMV(m, new b2Vec2(legW, 0)), legLen2);
                
                //bind upper leg 
                b2Utils.revoluteJoint(world, rect, triangle1, b2Math.MulMV(m, new b2Vec2(size.x*3/2, 0)), new b2Vec2); 
                
                //join motor
                b2Utils.distanceJoint(world, circle, triangle1, anchor, new b2Vec2(0, -legW), jointLen1);
                b2Utils.distanceJoint(world, circle, triangle2, anchor, new b2Vec2, jointLen2);
            }
        }
    }
}

有好的方法，或者有意思的代碼，請告訴我~