cocos2d-x 画物理齿轮

正在做物理小游戏， 用到齿轮

Node *drawGear(float r1,float r2 ,int teeth)
{
    DrawNode *gear = DrawNode::create();
    
    float da=2.0*PI/teeth/4.0;
    
    PhysicsBody* gearBody = PhysicsBody::create();
    PhysicsMaterial gearMaterial(0.05, 0.5, 0.05f);
    gearBody->setGravityEnable(false);
    Vec2 polyPoint[4];
    float angle;
    for(int i = 0; i < teeth; i++)
    {
        int j = 3;
        angle= i * 2.0 * PI / teeth;

        Vec2 pos1 = Vec2(r1*cos(angle),r1*sin(angle));
        polyPoint[j] = pos1;
        //gear->drawPoint(pos1, 3, Color4F::RED);
        Vec2 pos2 = Vec2(r2 * cos(angle+da),r2 * sin(angle+da));
        polyPoint[--j] = pos2;
        //gear->drawPoint(pos2, 3, Color4F::BLUE);
        gear->drawSegment(pos1, pos2, 1, Color4F::BLACK);

        pos1 = Vec2(r2 * cos(angle+da),r2 * sin(angle+da));
        //polyPoint[++j] = pos1;
        //gear->drawPoint(pos1, 3, Color4F::YELLOW);
        pos2 = Vec2(r2 * cos(angle+2*da),r2 * sin(angle+2*da));
        //polyPoint[++j] = pos2;
        //gear->drawPoint(pos2, 3, Color4F::GREEN);
        gear->drawSegment(pos1, pos2, 1, Color4F::BLACK);
        
        pos1 = Vec2(r2 * cos(angle+2*da),r2 * sin(angle+2*da));
        polyPoint[--j] = pos1;
        //gear->drawPoint(pos1, 3, Color4F::GRAY);
        pos2 = Vec2(r1 * cos(angle+3*da),r1 * sin(angle+3*da));
        polyPoint[--j] = pos2;
        //gear->drawPoint(pos2, 3, Color4F::MAGENTA);
        gear->drawSegment(pos1, pos2 , 1, Color4F::BLACK);
        
        gear->drawCircle(Vec2::ZERO, r1, 0, 60, false, Color4F::BLACK);
        gearBody->addShape(PhysicsShapeCircle::create(r1,gearMaterial));
        gear->drawCircle(Vec2::ZERO, r1/3*2, 0, 60, false, Color4F::BLACK);
        gearBody->addShape(PhysicsShapePolygon::create(polyPoint, 4,gearMaterial));
    }
    gear->setPhysicsBody(gearBody);
    
    return gear;
}