Ragdoll Example

/*
** Example: Ragdoll.kl
*/

require Math;
require Bullet;
require Geometry;
require BulletHelpers;

const Integer BODYPART_PELVIS = 0;
const Integer BODYPART_SPINE = 1;

const Integer BODYPART_HEAD = 2;

const Integer BODYPART_LEFT_UPPER_LEG = 3;
const Integer BODYPART_LEFT_LOWER_LEG = 4;

const Integer BODYPART_RIGHT_UPPER_LEG = 5;
const Integer BODYPART_RIGHT_LOWER_LEG = 6;

const Integer BODYPART_LEFT_UPPER_ARM = 7;
const Integer BODYPART_LEFT_LOWER_ARM = 8;

const Integer BODYPART_RIGHT_UPPER_ARM = 9;
const Integer BODYPART_RIGHT_LOWER_ARM = 10;

const Integer BODYPART_COUNT = 11;


const Integer JOINT_PELVIS_SPINE = 0;
const Integer JOINT_SPINE_HEAD = 1;

const Integer JOINT_LEFT_HIP = 2;
const Integer JOINT_LEFT_KNEE = 3;

const Integer JOINT_RIGHT_HIP = 4;
const Integer JOINT_RIGHT_KNEE = 5;

const Integer JOINT_LEFT_SHOULDER = 6;
const Integer JOINT_LEFT_ELBOW = 7;

const Integer JOINT_RIGHT_SHOULDER = 8;
const Integer JOINT_RIGHT_ELBOW = 9;

const Integer JOINT_COUNT = 10;


object BulletRagdoll {
  Color color;
  BulletCollisionShape  shapes[];
  BulletRigidBody       bodies[];
  BulletTypedConstraint joints[];
};


function BulletRagdoll.addBodyPart!(io RigidBodySimulation sim, Integer index, Xfo xfo)
{
  BulletCapsuleShape shape = this.shapes[index];
  Lines lines();
  Scalar radius = shape.getRadius();
  Scalar height = shape.getHalfHeight() * 2.0;
  lines.addCapsule(Xfo(), radius, height, 24);

  this.bodies[index] = sim.addRigidBody(1.0, 0.5, 0.0, xfo, Vec3(), Vec3(), shape);
}

function BulletRagdoll (io RigidBodySimulation sim, Xfo offset, Color color)
{
  this.color = color;
  // Setup the geometry
  this.shapes.resize(BODYPART_COUNT);
  this.shapes[BODYPART_PELVIS] = BulletCapsuleShape(0.15, 0.20);
  this.shapes[BODYPART_SPINE] = BulletCapsuleShape(0.15, 0.28);
  this.shapes[BODYPART_HEAD] = BulletCapsuleShape(0.10, 0.05);
  this.shapes[BODYPART_LEFT_UPPER_LEG] = BulletCapsuleShape(0.07, 0.45);
  this.shapes[BODYPART_LEFT_LOWER_LEG] = BulletCapsuleShape(0.05, 0.37);
  this.shapes[BODYPART_RIGHT_UPPER_LEG] = BulletCapsuleShape(0.07, 0.45);
  this.shapes[BODYPART_RIGHT_LOWER_LEG] = BulletCapsuleShape(0.05, 0.37);
  this.shapes[BODYPART_LEFT_UPPER_ARM] = BulletCapsuleShape(0.05, 0.33);
  this.shapes[BODYPART_LEFT_LOWER_ARM] = BulletCapsuleShape(0.04, 0.25);
  this.shapes[BODYPART_RIGHT_UPPER_ARM] = BulletCapsuleShape(0.05, 0.33);
  this.shapes[BODYPART_RIGHT_LOWER_ARM] = BulletCapsuleShape(0.04, 0.25);

  // Setup all the rigid bodies
  this.bodies.resize(BODYPART_COUNT);

  Xfo transform;
  transform.setIdentity();
  transform.tr = Vec3(0.0, 1.0, 0.0);
  this.addBodyPart(sim, BODYPART_PELVIS, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(0.0, 1.2, 0.0);
  this.addBodyPart(sim, BODYPART_SPINE, offset*transform);


  transform.setIdentity();
  transform.tr = Vec3(0.0, 1.6, 0.0);
  this.addBodyPart(sim, BODYPART_HEAD, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(-0.18, 0.65, 0.0);
  this.addBodyPart(sim, BODYPART_LEFT_UPPER_LEG, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(-0.18, 0.2, 0.0);
  this.addBodyPart(sim, BODYPART_LEFT_LOWER_LEG, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(0.18, 0.65, 0.0);
  this.addBodyPart(sim, BODYPART_RIGHT_UPPER_LEG, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(0.18, 0.2, 0.0);
  this.addBodyPart(sim, BODYPART_RIGHT_LOWER_LEG, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(-0.35, 1.45, 0.0);
  transform.ori.setFromEuler(Euler(0.0, 0.0, HALF_PI, RotationOrder('XYZ')));
  this.addBodyPart(sim, BODYPART_LEFT_UPPER_ARM, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(-0.7, 1.45, 0.0);
  transform.ori.setFromEuler(Euler(0.0, 0.0, HALF_PI, RotationOrder('XYZ')));
  this.addBodyPart(sim, BODYPART_LEFT_LOWER_ARM, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(0.35, 1.45, 0.0);
  transform.ori.setFromEuler(Euler(0,0,-HALF_PI, RotationOrder('XYZ')));
  this.addBodyPart(sim, BODYPART_RIGHT_UPPER_ARM, offset*transform);

  transform.setIdentity();
  transform.tr = Vec3(0.7, 1.45, 0.0);
  transform.ori.setFromEuler(Euler(0,0,-HALF_PI, RotationOrder('XYZ')));
  this.addBodyPart(sim, BODYPART_RIGHT_LOWER_ARM, offset*transform);

  // Setup some damping on the this.bodies
  for (Integer i = 0; i < BODYPART_COUNT; ++i)
  {
    this.bodies[i].setDamping(0.05, 0.85);
    this.bodies[i].setDeactivationTime(0.8);
    this.bodies[i].setSleepingThresholds(1.6, 2.5);
  }

  // Now setup the constraints
  this.joints.resize(JOINT_COUNT);
  Xfo localA, localB;
  localA.setIdentity(); localB.setIdentity();
  {
    localA.ori.setFromEuler(Euler(0.0, HALF_PI, 0.0, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, 0.15, 0.0);
    localB.ori.setFromEuler(Euler(0.0, HALF_PI, 0.0, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.15, 0.0);
    BulletHingeConstraint hingeC(this.bodies[BODYPART_PELVIS], this.bodies[BODYPART_SPINE], localA, localB);//
    hingeC.setLimit(-QUARTER_PI, HALF_PI);
    this.joints[JOINT_PELVIS_SPINE] = hingeC;
    //hingeC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0,0, HALF_PI, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, 0.30, 0.0);
    localB.ori.setFromEuler(Euler(0,0, HALF_PI, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.14, 0.0);
    BulletConeTwistConstraint coneC(this.bodies[BODYPART_SPINE], this.bodies[BODYPART_HEAD], localA, localB);
    coneC.setLimit(QUARTER_PI, QUARTER_PI, HALF_PI);
    this.joints[JOINT_SPINE_HEAD] = coneC;
    //coneC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0,0,-QUARTER_PI*5, RotationOrder('XYZ'))); localA.tr = Vec3(-0.18, -0.10, 0.0);
    localB.ori.setFromEuler(Euler(0,0,-QUARTER_PI*5, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, 0.225, 0.0);
    BulletConeTwistConstraint coneC(this.bodies[BODYPART_PELVIS], this.bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
    coneC.setLimit(QUARTER_PI, QUARTER_PI, 0);
    this.joints[JOINT_LEFT_HIP] = coneC;
    //coneC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0.0, HALF_PI, 0.0, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, -0.225, 0.0);
    localB.ori.setFromEuler(Euler(0.0, HALF_PI, 0.0, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, 0.185, 0.0);
    BulletHingeConstraint hingeC(this.bodies[BODYPART_LEFT_UPPER_LEG], this.bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
    hingeC.setLimit(0, HALF_PI);
    this.joints[JOINT_LEFT_KNEE] = hingeC;
    //hingeC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0, 0, QUARTER_PI, RotationOrder('XYZ'))); localA.tr = Vec3(0.18, -0.10, 0.0);
    localB.ori.setFromEuler(Euler(0, 0, QUARTER_PI, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, 0.225, 0.0);
    BulletConeTwistConstraint coneC(this.bodies[BODYPART_PELVIS], this.bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
    coneC.setLimit(QUARTER_PI, QUARTER_PI, 0);
    this.joints[JOINT_RIGHT_HIP] = coneC;
    //coneC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, -0.225, 0.0);
    localB.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, 0.185, 0.0);
    BulletHingeConstraint hingeC = BulletHingeConstraint(this.bodies[BODYPART_RIGHT_UPPER_LEG], this.bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
    hingeC.setLimit(0, HALF_PI);
    this.joints[JOINT_RIGHT_KNEE] = hingeC;
    //hingeC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0,0, PI, RotationOrder('XYZ'))); localA.tr = Vec3(-0.2, 0.15, 0.0);
    localB.ori.setFromEuler(Euler(0,0, HALF_PI, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.18, 0.0);
    BulletConeTwistConstraint coneC(this.bodies[BODYPART_SPINE], this.bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
    coneC.setLimit(HALF_PI, HALF_PI, 0);
    this.joints[JOINT_LEFT_SHOULDER] = coneC;
    //coneC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }
  
  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, 0.18, 0.0);
    localB.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.14, 0.0);
    BulletHingeConstraint hingeC(this.bodies[BODYPART_LEFT_UPPER_ARM], this.bodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
  //    hingeC.setLimit(HALF_PI, 0));
    hingeC.setLimit(0, HALF_PI);
    this.joints[JOINT_LEFT_ELBOW] = hingeC;
    //hingeC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }


  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0,0,0, RotationOrder('XYZ'))); localA.tr = Vec3(0.2, 0.15, 0.0);
    localB.ori.setFromEuler(Euler(0,0, HALF_PI, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.18, 0.0);
    BulletConeTwistConstraint coneC(this.bodies[BODYPART_SPINE], this.bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
    coneC.setLimit(HALF_PI, HALF_PI, 0);
    this.joints[JOINT_RIGHT_SHOULDER] = coneC;
    //coneC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }

  {
    localA.setIdentity(); localB.setIdentity();
    localA.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localA.tr = Vec3(0.0, 0.18, 0.0);
    localB.ori.setFromEuler(Euler(0.0, HALF_PI,0.0, RotationOrder('XYZ'))); localB.tr = Vec3(0.0, -0.14, 0.0);
    BulletHingeConstraint hingeC(this.bodies[BODYPART_RIGHT_UPPER_ARM], this.bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
  //    hingeC.setLimit(HALF_PI, 0));
    hingeC.setLimit(0, HALF_PI);
    this.joints[JOINT_RIGHT_ELBOW] = hingeC;
    //hingeC.setDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
  }
  for(Integer i=0; i<this.joints.size(); i++){
    sim.dynamicsWorld.addConstraint(this.joints[i], true);
  }
}


operator entry(){

  RigidBodySimulation sim();
  sim.initPhysics();
  sim.createGround();
  Xfo offset;
  BulletRagdoll ragdoll = BulletRagdoll(sim, offset, Color());
  sim.initialized = true;

  // Xfo bodyXfos[];
  // bodyXfos.resize(ragdoll.bodies.size);

  for(Integer i=0; i<300; i++){
    sim.stepSimulation();

    // Report the bodies
    // for(Integer j=0; j<ragdoll.bodies.size; j++){
    //   ragdoll.bodies[j].getTransform(bodyXfos[j]);
    // }
    // report("bodyXfos"+i+":" + bodyXfos);
    
    if((i%10)==0){
      Xfo headXfo = ragdoll.bodies[BODYPART_HEAD].getWorldTransform();
      report("Head Xfo"+i+":" + unitTestPrint(headXfo.tr));
    }
  }
}

/*
** Output:

Head Xfo0:{x:+4.902482e-5,y:+1.6333,z:-1.629232e-7}
Head Xfo10:{x:+0.656127e-3,y:+1.675293,z:-1.113176e-3}
Head Xfo20:{x:+0.083023,y:+1.256591,z:-0.060401}
Head Xfo30:{x:+2.994918e-2,y:+1.03247,z:-0.216217}
Head Xfo40:{x:+0.070022,y:+0.919799,z:-0.493652}
Head Xfo50:{x:+0.258728,y:+0.428283,z:-0.887695}
Head Xfo60:{x:+0.467163,y:+0.100753,z:-1.185791}
Head Xfo70:{x:+0.756225,y:+0.097763,z:-1.356201}
Head Xfo80:{x:+0.810791,y:+0.098754,z:-1.337402}
Head Xfo90:{x:+0.811035,y:+0.101043,z:-1.343994}
Head Xfo100:{x:+0.809692,y:+0.101593,z:-1.351806}
Head Xfo110:{x:+0.809326,y:+0.10234,z:-1.358154}
Head Xfo120:{x:+0.810424,y:+0.102996,z:-1.362793}
Head Xfo130:{x:+0.810913,y:+0.103958,z:-1.366699}
Head Xfo140:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo150:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo160:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo170:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo180:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo190:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo200:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo210:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo220:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo230:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo240:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo250:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo260:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo270:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo280:{x:+0.811401,y:+0.104324,z:-1.367675}
Head Xfo290:{x:+0.811401,y:+0.104324,z:-1.367675}

*/