使用 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);
            }
        }
    }
}

有好的方法，或者有意思的代码，请告诉我~