void vpRagdollWorld::Create()
{
int i, j;
SetGravity(Vec3(0.0, 0.0, -10.0));
floor.m_szName = string("floor");
floor.SetGround();
floor.AddGeometry(new vpBox(Vec3(100.0, 100.0, 5.0)), Vec3(0.0, 0.0, -10.0));
AddBody(&floor);
for ( i = 0; i < NUM_PUPPET; i++ )
for ( j = 0; j < NUM_PUPPET; j++ )
{
puppet[i][j].Create(this);
//puppet[i][j].G.SetFrame(Vec3(12 * j, 8 * i, 0));
puppet[i][j].G.SetFrame(Vec3(12 * (j - NUM_PUPPET / 2), 8 * (i - NUM_PUPPET / 2), 0));
}
for ( i = 0; i < NUM_STONE; i++ )
{
stone[i].AddGeometry(new vpSphere(1));
AddBody(&stone[i]);
stone[i].SetFrame(Vec3(0, 3*i, -100));
//stone[i].SetInertia(Inertia(5.0));
//sprintf(stone[i].m_szName, "ball%i", i);
}
//EnableCollision(false);
//SetIntegrator(VP::IMPLICIT_EULER_FAST);
SetIntegrator(VP::EULER);
SetTimeStep(0.001);
Initialize();
renderer.SetTarget(this);
if ( pWoodMaterial )
for ( i = 0; i < GetNumBody(); i++ ) renderer.SetMaterial(GetBody(i), pWoodMaterial);
if ( pMarbleMaterial )
{
renderer.SetMaterial(&floor, pMarbleMaterial);
for ( i = 0; i < NUM_STONE; i++ ) renderer.SetMaterial(&stone[i], pMarbleMaterial);
for ( i = 0; i < NUM_PUPPET; i++ )
for ( j = 0; j < NUM_PUPPET; j++ )
for ( int k = 0; k < NUM_ROPE; k++ )
renderer.SetMaterial(&puppet[i][j].B_rope[k], pMarbleMaterial);
}
}
// ---------------------------------------------------------
update_status ModulePhysics3D::Update(float dt)
{
if(App->input->GetKey(SDL_SCANCODE_F1) == KEY_DOWN)
debug = !debug;
if(debug == true)
{
world->debugDrawWorld();
// Render vehicles
p2List_item<PhysVehicle3D*>* item = vehicles.getFirst();
while(item)
{
item->data->Render();
item = item->next;
}
if(App->input->GetKey(SDL_SCANCODE_1) == KEY_DOWN)
{
Sphere s(1);
s.SetPos(App->camera->Position.x, App->camera->Position.y, App->camera->Position.z);
float force = 30.0f;
AddBody(s)->Push(-(App->camera->Z.x * force), -(App->camera->Z.y * force), -(App->camera->Z.z * force));
}
}
return UPDATE_CONTINUE;
}
/*
================
idAF::SetBase
Sets the base body.
================
*/
void idAF::SetBase( idAFBody *body, const idJointMat *joints )
{
physicsObj.ForceBodyId( body, 0 );
baseOrigin = body->GetWorldOrigin();
baseAxis = body->GetWorldAxis();
AddBody( body, joints, animator->GetJointName( animator->GetFirstChild( "origin" ) ), AF_JOINTMOD_AXIS );
}
/*
================
idAF::SetBase
Sets the base body.
================
*/
void idAF::SetBase( idAFBody* body, const idJointMat* joints )
{
physicsObj.ForceBodyId( body, 0 );
baseOrigin = body->GetWorldOrigin();
baseAxis = body->GetWorldAxis();
AddBody( body, joints, baseAfJoint.c_str(), AF_JOINTMOD_AXIS );
}
void Space::GenBody(SystemBody *sbody, Frame *f)
{
Body *b = 0;
if (sbody->type != SystemBody::TYPE_GRAVPOINT) {
if (sbody->GetSuperType() == SystemBody::SUPERTYPE_STAR) {
Star *star = new Star(sbody);
b = star;
} else if ((sbody->type == SystemBody::TYPE_STARPORT_ORBITAL) ||
(sbody->type == SystemBody::TYPE_STARPORT_SURFACE)) {
SpaceStation *ss = new SpaceStation(sbody);
b = ss;
} else {
Planet *planet = new Planet(sbody);
b = planet;
}
b->SetLabel(sbody->name.c_str());
b->SetPosition(vector3d(0,0,0));
AddBody(b);
}
f = MakeFrameFor(sbody, b, f);
for (std::vector<SystemBody*>::iterator i = sbody->children.begin(); i != sbody->children.end(); ++i) {
GenBody(*i, f);
}
}
/*!
Resets the Simdata and creates a new description of the body based on the
Body from the Creature.
\param blueprint is the Creature base from which to create the
BulletCreature.
\param x_displacement is used for positioning the creature with a
displacement in the x-axis.
*/
BulletCreature::BulletCreature(Creature blueprint, float x_displacement) {
//connect brain
blueprint_ = blueprint;
//create body
BodyTree body_root = blueprint_.GetBody().GetBodyRoot();
AddBody(body_root, btVector3(x_displacement,-body_root.GetLowestPoint(),0.0));
brain_counter_ = 1;
}
ssize_t CHttpMessage::SetBody(const char* apcBuf, size_t aiBufLen)
{
ssize_t liLen = 0;
ClearBody();
liLen = AddBody(apcBuf, aiBufLen);
return liLen;
}
/*!
Resets the Simdata and creates a new description of the body based on the
Body from the Creature.
\param blueprint is the Creature base from which to create the
BulletCreature.
*/
BulletCreature::BulletCreature(Creature blueprint) {
//connect brain
blueprint_ = blueprint;
blueprint_.simdata.ResetData();
//create body
BodyTree body_root = blueprint_.GetBody().GetBodyRoot();
AddBody(body_root, btVector3(0.0,-body_root.GetLowestPoint(),0.0));
brain_counter_ = 1;
}
void URDF_RBDL_Model::process(const urdf::Link& link, unsigned int parent, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
if(link.inertial)
{
const urdf::Inertial& in = *link.inertial;
Math::Matrix3d I;
I << in.ixx, in.ixy, in.ixz,
in.ixy, in.iyy, in.iyz,
in.ixz, in.iyz, in.izz;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, toRBDL(link.inertial->origin.position), I);
}
else
{
body.mCenterOfMass = Math::Vector3d::Zero();
body.mInertia = Math::Matrix3d::Zero();
body.mMass = 0.000001;
}
Math::SpatialTransform jointFrame = Math::SpatialTransform();
RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
if(link.parent_joint)
{
jointFrame = toRBDL(link.parent_joint->parent_to_joint_origin_transform);
jointFrame.r = jointFrame.r - parentJointFrame.r;
joint = toRBDL(*link.parent_joint);
}
// Add the body to the RBDL tree (Note: This function merges fixed joints into following non-fixed joints and doesn't create a new id for them)
unsigned int id = AddBody(parent, jointFrame, joint, body, link.name);
#if DISPLAY_DEBUG_INFO
ROS_INFO("Added body link '%s' as ID %u, Parent ID %u, Joint type %d, Mass %.4f (now %d DOF)", link.name.c_str(), id, parent, joint.mJointType, body.mMass, dof_count);
#endif
// Set up the associated joint if required
if(link.parent_joint && link.parent_joint->type != urdf::Joint::FIXED)
{
setupJoint(id, *link.parent_joint, false);
#if DISPLAY_DEBUG_INFO
ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, link.parent_joint->name.c_str(), link.parent_joint->parent_link_name.c_str(), link.parent_joint->child_link_name.c_str());
#endif
}
if(link.name == m_nameURDFRoot)
m_indexURDFRoot = (dof_count == 0 ? 0 : id);
for(size_t i = 0; i < link.child_links.size(); ++i)
process(*link.child_links[i], id, Math::SpatialTransform());
}
void GameLayer::update(float dt){
if (!HeadCollideBody(head->getDirec()) && !OutofRange()) {
MoveStep();
if(ifGetFood()){
AddBody();
SetFood();
}
lastbodyposi = body.at(body.size()-1)->getPosition();
}
else{
GameOver();
}
}
void Snake::Begin()
{
size = 0;
Body = (Voxel**)malloc(sizeof(Voxel*)*MaxLength);
Cube->CreateCubeOff();
Cur_x = random()%8;
Cur_y = random()%8;
Cur_z = random()%8;
AddBody(Cur_x,Cur_y,Cur_z);
Reset = !CalculateDirection();
StartTime = millis();
}
void URDF_RBDL_Model::process(const urdf::Link& link, int parent, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
if(link.inertial)
{
const urdf::Inertial& in = *link.inertial;
Math::Matrix3d I;
I << in.ixx, in.ixy, in.ixz,
in.ixy, in.iyy, in.iyz,
in.ixz, in.iyz, in.izz;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, toRBDL(link.inertial->origin.position), I);
}
else
{
body.mCenterOfMass = Math::Vector3d::Zero();
body.mInertia = Math::Matrix3d::Zero();
body.mMass = 0.000001;
}
Math::SpatialTransform jointFrame = Math::SpatialTransform();
RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
if(link.parent_joint)
{
jointFrame = toRBDL(link.parent_joint->parent_to_joint_origin_transform);
jointFrame.r = jointFrame.r - parentJointFrame.r;
joint = toRBDL(*link.parent_joint);
}
int id = AddBody(parent, jointFrame, joint, body, link.name);
if(link.parent_joint && link.parent_joint->type != urdf::Joint::FIXED)
setupJoint(id, *link.parent_joint, false);
for(size_t i = 0; i < link.child_links.size(); ++i)
{
process(*link.child_links[i], id, Math::SpatialTransform());
}
}
// ---------------------------------------------------------
update_status ModulePhysics3D::Update(float dt)
{
if(App->input->GetKey(SDL_SCANCODE_F1) == KEY_DOWN)
debug = !debug;
if(debug == true)
{
world->debugDrawWorld();
if(App->input->GetKey(SDL_SCANCODE_1) == KEY_DOWN)
{
Sphere s(1);
s.SetPos(App->camera->Position.x, App->camera->Position.y, App->camera->Position.z);
AddBody(s);
}
}
return UPDATE_CONTINUE;
}
void Snake::Update()
{
if(!Reset && millis() > StartTime + Delay)
{
StartTime = millis();
// while not in range, and selected voxel is not currently turned on
if(Length <= 0 || !Cube->InRange(Cur_x + Dir_x,Cur_y + Dir_y,Cur_z + Dir_z) ||Cube->GetVoxel(Cur_x + Dir_x,Cur_y + Dir_y,Cur_z + Dir_z) == 1)
{
Reset = !CalculateDirection();
}
Cur_x += Dir_x;
Cur_y += Dir_y;
Cur_z += Dir_z;
Length--;
AddBody(Cur_x,Cur_y,Cur_z);
}
else if(Reset && millis()-(Delay+2000) > StartTime)
{
DeleteArray();
Begin();
}
}
void URDF_RBDL_Model::processReverse(const urdf::Link& link, unsigned int parent, const urdf::Link* parentLink, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
// Situation here: In the URDF world, we are the child of link.parent_link.
// We want to become the child of parentLink, which is a child of ours in
// URDF.
// Our origin needs to be in the point of the joint from parentLink to us.
// Determine transform from old origin to the new origin (joint of the parentLink)
Math::SpatialTransform tf;
boost::shared_ptr<urdf::Joint> child_joint;
if(parentLink)
{
child_joint = parentLink->parent_joint;
tf = toRBDL(child_joint->parent_to_joint_origin_transform);
}
else
{
// Just leave our origin at the child joint
tf.E = Math::Matrix3d::Identity();
tf.r = Math::Vector3d::Zero();
}
if(link.inertial)
{
const urdf::Inertial& in = *link.inertial;
Math::Matrix3d I;
I << in.ixx, in.ixy, in.ixz,
in.ixy, in.iyy, in.iyz,
in.ixz, in.iyz, in.izz;
Math::Vector3d cog = toRBDL(link.inertial->origin.position);
// As the coordinate system is not rotated, and the center of mass
// stays where it is, the inertia tensor does not need to be
// modified.
// Transform cog relative to our new origin
cog = cog - tf.r;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, cog, I);
}
else
{
body.mCenterOfMass = Math::Vector3d::Zero();
body.mInertia = Math::Matrix3d::Zero();
body.mMass = 0.000001;
}
Math::SpatialTransform jointFrame = Math::SpatialTransform();
RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
if(parentLink && parentLink->parent_joint)
{
// Rotate the other way around since we are reversing the joint.
Math::Matrix3d E = tf.E.transpose();
// Origin of the joint in parentLink coordinates
Math::Vector3d r = -parentJointFrame.r;
jointFrame = Math::SpatialTransform(E, r);
joint = toRBDL(*parentLink->parent_joint);
// Flip joint axes
for(size_t i = 0; i < joint.mDoFCount; ++i)
joint.mJointAxes[i] = -joint.mJointAxes[i];
}
// Add the body to the RBDL tree (Note: This function merges fixed joints into following non-fixed joints and doesn't create a new id for them)
unsigned int id = AddBody(parent, jointFrame, joint, body, "?reverse_" + link.name);
#if DISPLAY_DEBUG_INFO
ROS_INFO("Added body link '%s' as ID %u, Parent ID %u, Joint type %d, Mass %.4f (now %d DOF)", link.name.c_str(), id, parent, joint.mJointType, body.mMass, dof_count);
ROS_INFO(" -> parentLink: %p, parentLink->parentJoint: %p",
parentLink, parentLink ? parentLink->parent_joint.get() : 0
);
ROS_INFO(" -> joint frame was %f, %f, %f", jointFrame.r.x(), jointFrame.r.y(), jointFrame.r.z());
if(id >= fixed_body_discriminator)
{
ROS_INFO(" -> fixed body has movable parent %u now, offset %f, %f, %f",
mFixedBodies[id - fixed_body_discriminator].mMovableParent,
mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.x(),
mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.y(),
mFixedBodies[id - fixed_body_discriminator].mParentTransform.r.z()
);
}
#endif
// Set up the associated joint if required
if(parentLink && parentLink->parent_joint && parentLink->parent_joint->type != urdf::Joint::FIXED)
{
setupJoint(id, *parentLink->parent_joint, true);
#if DISPLAY_DEBUG_INFO
ROS_WARN("Added joint %u: '%s' (%s -> %s)", id - 1, parentLink->parent_joint->name.c_str(), parentLink->parent_joint->parent_link_name.c_str(), parentLink->parent_joint->child_link_name.c_str());
#endif
}
if(link.name == m_nameURDFRoot)
m_indexURDFRoot = (dof_count == 0 ? 0 : id);
// Provide a zero-weight fixed body with the correct origin
{
RigidBodyDynamics::Joint fixedJoint(RigidBodyDynamics::JointTypeFixed);
RigidBodyDynamics::Body fixedBody;
fixedBody.mMass = 0.0;
RigidBodyDynamics::Math::SpatialTransform t;
t.E.setIdentity();
t.r = -tf.r;
AddBody(id, t, fixedJoint, fixedBody, link.name);
}
for(size_t i = 0; i < link.child_links.size(); ++i)
{
const urdf::Link* child = link.child_links[i].get();
if(child == parentLink)
continue;
process(*link.child_links[i], id, tf);
}
boost::shared_ptr<urdf::Link> urdfParent = link.getParent();
if(urdfParent)
processReverse(*urdfParent, id, &link, tf);
}
/*
================
idAF::LoadBody
================
*/
bool idAF::LoadBody( const idDeclAF_Body *fb, const idJointMat *joints )
{
int id, i;
float length, mass;
idTraceModel trm;
idClipModel *clip;
idAFBody *body;
idMat3 axis, inertiaTensor;
idVec3 centerOfMass, origin;
idBounds bounds;
idList<jointHandle_t> jointList;
origin = fb->origin.ToVec3();
axis = fb->angles.ToMat3();
bounds[0] = fb->v1.ToVec3();
bounds[1] = fb->v2.ToVec3();
switch( fb->modelType )
{
case TRM_BOX:
{
trm.SetupBox( bounds );
break;
}
case TRM_OCTAHEDRON:
{
trm.SetupOctahedron( bounds );
break;
}
case TRM_DODECAHEDRON:
{
trm.SetupDodecahedron( bounds );
break;
}
case TRM_CYLINDER:
{
trm.SetupCylinder( bounds, fb->numSides );
break;
}
case TRM_CONE:
{
// place the apex at the origin
bounds[0].z -= bounds[1].z;
bounds[1].z = 0.0f;
trm.SetupCone( bounds, fb->numSides );
break;
}
case TRM_BONE:
{
// direction of bone
axis[2] = fb->v2.ToVec3() - fb->v1.ToVec3();
length = axis[2].Normalize();
// axis of bone trace model
axis[2].NormalVectors( axis[0], axis[1] );
axis[1] = -axis[1];
// create bone trace model
trm.SetupBone( length, fb->width );
break;
}
default:
assert( 0 );
break;
}
trm.GetMassProperties( 1.0f, mass, centerOfMass, inertiaTensor );
trm.Translate( -centerOfMass );
origin += centerOfMass * axis;
body = physicsObj.GetBody( fb->name );
if( body )
{
clip = body->GetClipModel();
if( !clip->IsEqual( trm ) )
{
clip = new idClipModel( trm );
clip->SetContents( fb->contents );
clip->Link( gameLocal.clip, self, 0, origin, axis );
body->SetClipModel( clip );
}
clip->SetContents( fb->contents );
body->SetDensity( fb->density, fb->inertiaScale );
body->SetWorldOrigin( origin );
body->SetWorldAxis( axis );
id = physicsObj.GetBodyId( body );
}
else
{
clip = new idClipModel( trm );
clip->SetContents( fb->contents );
clip->Link( gameLocal.clip, self, 0, origin, axis );
body = new idAFBody( fb->name, clip, fb->density );
if( fb->inertiaScale != mat3_identity )
{
body->SetDensity( fb->density, fb->inertiaScale );
}
id = physicsObj.AddBody( body );
}
if( fb->linearFriction != -1.0f )
{
body->SetFriction( fb->linearFriction, fb->angularFriction, fb->contactFriction );
}
body->SetClipMask( fb->clipMask );
body->SetSelfCollision( fb->selfCollision );
if( fb->jointName == "origin" )
{
SetBase( body, joints );
}
else
{
AFJointModType_t mod;
if( fb->jointMod == DECLAF_JOINTMOD_AXIS )
{
mod = AF_JOINTMOD_AXIS;
}
else if( fb->jointMod == DECLAF_JOINTMOD_ORIGIN )
{
mod = AF_JOINTMOD_ORIGIN;
}
else if( fb->jointMod == DECLAF_JOINTMOD_BOTH )
{
mod = AF_JOINTMOD_BOTH;
}
else
{
mod = AF_JOINTMOD_AXIS;
}
AddBody( body, joints, fb->jointName, mod );
}
if( fb->frictionDirection.ToVec3() != vec3_origin )
{
body->SetFrictionDirection( fb->frictionDirection.ToVec3() );
}
if( fb->contactMotorDirection.ToVec3() != vec3_origin )
{
body->SetContactMotorDirection( fb->contactMotorDirection.ToVec3() );
}
// update table to find the nearest articulated figure body for a joint of the skeletal model
animator->GetJointList( fb->containedJoints, jointList );
for( i = 0; i < jointList.Num(); i++ )
{
if( jointBody[ jointList[ i ] ] != -1 )
{
gameLocal.DWarning( "%s: joint '%s' is already contained by body '%s'",
name.c_str(), animator->GetJointName( ( jointHandle_t )jointList[i] ),
physicsObj.GetBody( jointBody[ jointList[ i ] ] )->GetName().c_str() );
}
jointBody[ jointList[ i ] ] = id;
}
return true;
}
/*!
This function is recursive and first called for the root of the tree (the
head of the creature). In this function, the strength of the joints are
determined by the masses of the connected BodyTrees if they are not
explicitly set.
\param body is the BodyTree from which to setup the rigidbody used by Bullet
in the simulation.
\param position is the position of where in the physics world to put the
current body.
*/
btRigidBody* BulletCreature::AddBody(BodyTree body, btVector3 position) {
//shape
btVector3 dim = btVector3(body.box_dim.x,body.box_dim.y,body.box_dim.z);
btCollisionShape* shape = new btBoxShape(dim);
btVector3 fallInertia(0,0,0);
float mass = body.GetMass();
shape->calculateLocalInertia(mass,fallInertia);
// Material
Material current_material = body.material;
//body
btTransform offset;
offset.setIdentity();
offset.setOrigin(position);
btMotionState* motion_state;
motion_state = new btDefaultMotionState(offset);
btRigidBody::btRigidBodyConstructionInfo rigid_body(mass,motion_state,shape,fallInertia);
btRigidBody* current_body = new btRigidBody(rigid_body);
current_body->setFriction(body.friction);
// Add data
m_bodies_.push_back(current_body);
materials_.push_back(current_material);
mass_.push_back(mass);
m_shapes_.push_back(shape);
//add children
Joint joint;
btTransform localA, localB;
for(int i=0; i<body.body_list.size(); i++) {
//child position
joint = body.body_list[i].root_joint;
Vec3 c = joint.connection_root - joint.connection_branch;
btVector3 child_offset(c.x,c.y,c.z);
//body
btRigidBody* child_body = AddBody(body.body_list[i],position+child_offset);
//joint
localA.setIdentity();
localB.setIdentity();
Vec3 h = joint.hinge_orientation;
localA.getBasis().setEulerZYX(h.x,h.y,h.z);
localB.getBasis().setEulerZYX(h.x,h.y,h.z);
Vec3 a = joint.connection_root;
Vec3 b = joint.connection_branch;
localA.setOrigin(btVector3(a.x,a.y,a.z));
localB.setOrigin(btVector3(b.x,b.y,b.z));
btHingeConstraint* hinge = new btHingeConstraint(*(current_body), *(child_body), localA, localB);
m_joints_.push_back(hinge);
// For now just proportional to the mass of the bodies,
// seems to be what works best for most creatures.
float strength = (body.body_list[i].root_joint.strength < 0) ?
body.GetMass() + body.body_list[i].GetMass() :
body.body_list[i].root_joint.strength;
joint_strength_.push_back(strength);
m_joints_.back()->setLimit(
btScalar(joint.lower_limit),
btScalar(joint.upper_limit),
0.9, // Softness = 0.9 default
0.3, // Bias factor = 0.3 default
1.0); // Relaxation factor = 1.0 default
}
return current_body;
}
void vpSiloWorld::Create(void)
{
vpMaterial::GetDefaultMaterial()->SetRestitution(0.3);
vpMaterial::GetDefaultMaterial()->SetDynamicFriction(0.3);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
block[i][j].AddGeometry(new vpBox(Vec3(1, 2, 1)));
block[i][j].SetFrame(Exp(se3(0, 0, (scalar)(j + (scalar)0.5 * (i % 2)) / (scalar)NUM_SILO_BLOCKS * M_2PI, 0, 0, 0)) * SE3(Vec3(4.4, 0, 1.01 * i)));
AddBody(&block[i][j]);
block[i][j].SetInertia(Inertia(1));
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
ball[i].AddGeometry(new vpSphere(0.25));
ball[i].SetFrame(Vec3(drand(2.0), drand(2.0), NUM_SILO_LEVEL + (25.0 / NUM_BALL) * i));
ball[i].SetGenVelocity(se3(0,0,0,drand(0.1), drand(0.1), drand(0.1)));
ball[i].SetInertia(Inertia(0.5));
AddBody(&ball[i]);
}
floor.AddGeometry(new vpBox(Vec3(100, 100, 1)));
floor.SetFrame(Vec3(0, 0, -1.0));
floor.SetGround();
AddBody(&floor);
roof.AddGeometry(new vpBox(Vec3(10, 10, 1)), Vec3(0.0, 0.0, -1));
roof.SetFrame(Vec3(0, 0, -10));
AddBody(&roof);
//SetIntegrator(VP::IMPLICIT_EULER);
SetIntegrator(VP::EULER);
SetTimeStep(0.005);
SetGravity(Vec3(0.0, 0.0, -10.0));
Initialize();
BackupState();
renderer.SetTarget(this);
if ( materialArray.size() )
{
renderer.SetMaterial(&floor, materialArray[0]);
renderer.SetMaterial(&roof, materialArray[1]);
for ( int i = 0; i < NUM_SILO_LEVEL; i++ )
{
for ( int j = 0; j < NUM_SILO_BLOCKS; j++ )
{
renderer.SetMaterial(&block[i][j], materialArray[2]);
}
}
for ( int i = 0; i < NUM_BALL; i++ )
{
renderer.SetMaterial(&ball[i], materialArray[1]);
}
}
}
void URDF_RBDL_Model::processReverse(const urdf::Link& link, int parent, const urdf::Link* parentLink, const Math::SpatialTransform& parentJointFrame)
{
RigidBodyDynamics::Body body;
// Situation here: In the URDF world, we are the child of link.parent_link.
// We want to become the child of parentLink, which is a child of ours in
// URDF.
// Our origin needs to be in the point of the joint from parentLink to us.
// Determine transform from old origin to the new origin (joint of the parentLink)
Math::SpatialTransform tf;
boost::shared_ptr<urdf::Joint> child_joint;
if(parentLink)
{
child_joint = parentLink->parent_joint;
tf = toRBDL(child_joint->parent_to_joint_origin_transform);
}
else
{
// Just leave our origin at the child joint
tf.E = Math::Matrix3d::Identity();
tf.r = Math::Vector3d::Zero();
}
if(link.inertial)
{
const urdf::Inertial& in = *link.inertial;
Math::Matrix3d I;
I << in.ixx, in.ixy, in.ixz,
in.ixy, in.iyy, in.iyz,
in.ixz, in.iyz, in.izz;
Math::Vector3d cog = toRBDL(link.inertial->origin.position);
// As the coordinate system is not rotated, and the center of mass
// stays where it is, the inertia tensor does not need to be
// modified.
// Transform cog relative to our new origin
cog = cog - tf.r;
double mass = link.inertial->mass;
if(mass == 0)
mass = 0.000001;
body = RigidBodyDynamics::Body(mass, cog, I);
}
else
{
body.mCenterOfMass = Math::Vector3d::Zero();
body.mInertia = Math::Matrix3d::Zero();
body.mMass = 0.00001;
}
Math::SpatialTransform jointFrame = Math::SpatialTransform();
RigidBodyDynamics::Joint joint = RigidBodyDynamics::Joint(RigidBodyDynamics::JointTypeFixed);
if(parentLink && parentLink->parent_joint)
{
// Rotate the other way around since we are reversing the joint.
Math::Matrix3d E = tf.E.transpose();
// Origin of the joint in parentLink coordinates
Math::Vector3d r = -parentJointFrame.r;
jointFrame = Math::SpatialTransform(E, r);
joint = toRBDL(*parentLink->parent_joint);
// Flip joint axes
for(size_t i = 0; i < joint.mDoFCount; ++i)
{
joint.mJointAxes[i] = -joint.mJointAxes[i];
}
}
int id = AddBody(parent, jointFrame, joint, body, link.name);
if(parentLink && parentLink->parent_joint && parentLink->parent_joint->type != urdf::Joint::FIXED)
setupJoint(id, *parentLink->parent_joint, true);
for(size_t i = 0; i < link.child_links.size(); ++i)
{
const urdf::Link* child = link.child_links[i].get();
if(child == parentLink)
continue;
process(*link.child_links[i], id, tf);
}
boost::shared_ptr<urdf::Link> urdfParent = link.getParent();
if(urdfParent)
{
processReverse(*urdfParent, id, &link, tf);
}
}
