void capsule::create_physical_body(
double x,
double y,
double z,
double radius,
double length,
double mass,
manager& mgr)
{
this->radius = radius;
this->length = length;
world_id = mgr.ode_world();
space_id = mgr.ode_space();
//set the body orientation
// dMatrix3 R;
//dRFromAxisAndAngle(R,1,0,0,M_PI/2);
//dBodySetRotation(body_id,R);
//create the geom
geom_id=dCreateCapsule(mgr.ode_space(),radius,length);
object::set_geom_data(geom_id); //must make sure to set the geom data for the collision callback!
dGeomSetPosition (geom_id, x, y, z);
if(mass > 0)
{
object::create_rigid_body(x, y, z, mgr);
set_mass(mass);
dGeomSetBody(geom_id,body_id);
//create an amotor to keep the body vertical
amotor_id=dJointCreateAMotor(mgr.ode_world(),0);
dJointAttach(amotor_id,body_id,0);
dJointSetAMotorMode(amotor_id,dAMotorEuler);
dJointSetAMotorNumAxes(amotor_id,3);
dJointSetAMotorAxis(amotor_id,0,0,1,0,0);
dJointSetAMotorAxis(amotor_id,1,0,0,1,0);
dJointSetAMotorAxis(amotor_id,2,0,0,0,1);
dJointSetAMotorAngle(amotor_id,0,0);
dJointSetAMotorAngle(amotor_id,1,0);
dJointSetAMotorAngle(amotor_id,2,0);
dJointSetAMotorParam(amotor_id,dParamLoStop,-0);
dJointSetAMotorParam(amotor_id,dParamLoStop3,-0);
dJointSetAMotorParam(amotor_id,dParamLoStop2,-0);
dJointSetAMotorParam(amotor_id,dParamHiStop,0);
dJointSetAMotorParam(amotor_id,dParamHiStop3,0);
dJointSetAMotorParam(amotor_id,dParamHiStop2,0);
}
}
void PhysicsAMotorJoint::setAngle(Int32 anum, Real32 angle)
{
dJointSetAMotorAngle(_JointID, anum, angle);
}
