void Simulator::GetCurrentState(double s[])
{
/*
* State for each body consists of position, orientation (stored as a quaternion),
* linear velocity, and angular velocity. In the current robot model,
* the bodies are vehicle chassis and the vehicle wheels.
*/
const int n = (int) m_robotBodies.size();
for (int i = 0; i < n; ++i)
{
const dReal *pos = dBodyGetPosition (m_robotBodies[i]);
const dReal *rot = dBodyGetQuaternion(m_robotBodies[i]);
const dReal *vel = dBodyGetLinearVel (m_robotBodies[i]);
const dReal *ang = dBodyGetAngularVel(m_robotBodies[i]);
const int offset = 13 * i;
s[offset ] = pos[0];
s[offset + 1] = pos[1];
s[offset + 2] = pos[2];
s[offset + 3] = rot[0];
s[offset + 4] = rot[1];
s[offset + 5] = rot[2];
s[offset + 6] = rot[3];
s[offset + 7] = vel[0];
s[offset + 8] = vel[1];
s[offset + 9] = vel[2];
s[offset + 10] = ang[0];
s[offset + 11] = ang[1];
s[offset + 12] = ang[2];
}
s[13 * n] = dRandGetSeed();
}
void SParts::getAngularVelocity(Vector3d &v)
{
const dReal *avel = dBodyGetAngularVel(m_odeobj->body());
v.x(avel[0]);
v.y(avel[1]);
v.z(avel[2]);
}
/* returns the total kinetic energy of a body */
t_real body_total_kinetic_energy (dBodyID b) {
const t_real *v, *omega;
dVector3 wb;
t_real trans_energy, rot_energy, energy;
dMass m;
/* get the linear and angular velocities */
dBodyGetMass (b, &m);
v = dBodyGetLinearVel (b);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
/* and assign the energy */
/* TODO: restore */
/* energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
energy += 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]); */
trans_energy = 0.5 * m.mass * (v [0] * v [0] + v [1] * v [1] + v [2] * v [2]);
rot_energy = 0.5 * (m.I [0] * wb[0] * wb[0] + m.I[5] * wb[1] * wb[1] + m.I[10] * wb[2] * wb[2]);
energy = trans_energy + rot_energy;
/* printf ("trans: %.3f rot: %.3f\n", trans_energy, rot_energy); */
/* and return */
return energy;
}
void Gyroscope::GyroscopeSensor::updateValue()
{
const dReal* angularVelInWorld = dBodyGetAngularVel(body->body);
dVector3 result;
dBodyVectorFromWorld(body->body, angularVelInWorld[0], angularVelInWorld[1], angularVelInWorld[2], result);
ODETools::convertVector(result, angularVel);
}
void ompl::control::OpenDEStateSpace::readState(base::State *state) const
{
auto *s = state->as<StateType>();
for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
{
unsigned int _i4 = i * 4;
const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
for (int j = 0; j < 3; ++j)
{
s_pos[j] = pos[j];
s_vel[j] = vel[j];
s_ang[j] = ang[j];
}
const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
s_rot.w = rot[0];
s_rot.x = rot[1];
s_rot.y = rot[2];
s_rot.z = rot[3];
}
s->collision = 0;
}
void Object::UpdateDisableState()
{
bool disabled = true;
if (iBody == 0)
return;
if (dBodyIsEnabled(iBody) == 0)
return;
const dReal *lv = dBodyGetLinearVel(iBody);
const dReal *av = dBodyGetAngularVel(iBody);
if ((lv[0]*lv[0]+lv[1]*lv[1]+lv[2]*lv[2]) > DISABLE_THRESHOLD)
disabled = false;
if ((av[0]*av[0]+av[1]*av[1]+av[2]*av[2]) > DISABLE_THRESHOLD)
disabled = false;
if (disabled = false)
disabledSteps++;
if (disabledSteps > AUTO_DISABLE_STEPS)
{
dBodyDisable(iBody);
}
};
ngl::Vec3 RigidBody::getAngularVelocity()const
{
// When getting, the returned values are pointers to internal data structures,
// so the vectors are valid until any changes are made to the rigid body
// system structure.
const dReal *pos=dBodyGetAngularVel(m_id);
return ngl::Vec3(*pos,*(pos+1),*(pos+2));
}
void PSteerable::steeringToOde()
{
if (steerInfo.acc.length() != 0 || steerInfo.rot)
dBodyEnable(body);
Vec3f f = steerInfo.acc * mass.mass;
const dReal *angVel = dBodyGetAngularVel(body);
dBodySetAngularVel(body, angVel[0], angVel[1] + steerInfo.rot,
angVel[2]);
dBodyAddForce(body, f[0], f[1], f[2]);
}
/* returns the rotational z kinetic energy of a body */
t_real body_zrot_kinetic_energy (dBodyID b) {
dMass m;
const t_real *omega;
dVector3 wb;
dBodyGetMass (b, &m);
omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega [0], omega [1], omega [2], wb);
return 0.5*m.I[10]*wb[2]*wb[2];
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
bool Primitive::limitAngularVel(double maxVel){
// check for maximum speed:
if(!body) return false;
const double* vel = dBodyGetAngularVel( body );
double vellen = vel[0]*vel[0]+vel[1]*vel[1]+vel[2]*vel[2];
if(vellen > maxVel*maxVel){
fprintf(stderr, ".");
numVelocityViolations++;
globalNumVelocityViolations++;
double scaling = sqrt(vellen)/maxVel;
dBodySetAngularVel(body, vel[0]/scaling, vel[1]/scaling, vel[2]/scaling);
return true;
}else
return false;
}
static void simLoop (int pause)
{
if (!pause) {
dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
dWorldStep (world,0.03);
iteration++;
if (iteration >= 100) {
// measure the difference between the anchor and test bodies
const dReal *w1 = dBodyGetAngularVel (anchor_body);
const dReal *w2 = dBodyGetAngularVel (test_body);
const dReal *q1 = dBodyGetQuaternion (anchor_body);
const dReal *q2 = dBodyGetQuaternion (test_body);
dReal maxdiff = dMaxDifference (w1,w2,1,3);
printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
maxdiff = dMaxDifference (q1,q2,1,4);
printf ("q-error = %.4e\n",maxdiff);
reset_test();
}
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
dsSetColor (1,1,1);
dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
sides3);
dsSetColor (1,0,0);
dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawBox (dBodyGetPosition (particle[i]),
dBodyGetRotation (particle[i]), sides);
}
void odeBodyDamp( dBodyID dBody, float friction ) {
dReal *lv = (dReal *)dBodyGetLinearVel ( dBody );
dReal lv1[3];
lv1[0] = lv[0] * -friction;
lv1[1] = lv[1] * -friction;
lv1[2] = lv[2] * -friction;
dBodyAddForce( dBody, lv1[0], lv1[1], lv1[2] );
dReal *av = (dReal *)dBodyGetAngularVel( dBody );
dReal av1[3];
av1[0] = av[0] * -friction;
av1[1] = av[1] * -friction;
av1[2] = av[2] * -friction;
dBodyAddTorque( dBody, av1[0], av1[1], av1[2] );
}
void CPHIsland::Repair()
{
if(!m_flags.is_active()) return;
dBodyID body;
for (body = firstbody; body; body = (dxBody *) body->next)
{
if(!dV_valid(dBodyGetAngularVel(body)))
dBodySetAngularVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetLinearVel(body)))
dBodySetLinearVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetPosition(body)))
dBodySetPosition(body,0.f,0.f,0.f);
if(!dQ_valid(dBodyGetQuaternion(body)))
{
dQuaternion q={1.f,0.f,0.f,0.f};//dQSetIdentity(q);
dBodySetQuaternion(body,q);
}
}
}
/*** シミュレーションループ ***/
static void simLoop(int pause)
{
if (!pause)
{
dSpaceCollide(space,0,&nearCallback); // add
control();
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup); // add
}
const dReal *linear_vel = dBodyGetLinearVel(base.body);
const dReal *angular_vel = dBodyGetAngularVel(base.body);
printf("linear : %.3f %.3f %.3f\n", linear_vel[0], linear_vel[1], linear_vel[2]);
printf("angular: %.3f %.3f %.3f\n", angular_vel[0], angular_vel[1], angular_vel[2]);
drawBase();
drawWheel(); // add
drawBall(); //add
drawGoal();
}
void checktest11(unsigned long timer)
{
if (timer>1500)
{
Vehicle *_b = entities[0];
Vec3f val = _b->getPos()-Vec3f(-400 kmf,20.5f,-400 kmf);
dReal *v = (dReal *)dBodyGetLinearVel(_b->getBodyID());
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
dReal *av = (dReal *)dBodyGetAngularVel(_b->getBodyID());
Vec3f vav;
vav[0]= av[0];vav[1] = av[1]; vav[2] = av[2];
if (val.magnitude()>100)
{
printf("Test failed.\n");
endWorldModelling();
exit(-1);
} else if (vav.magnitude()>10)
{
printf("Test failed.\n");
endWorldModelling();
exit(-1);
} else if (vec3fV.magnitude()>5)
{
printf("Test failed.\n");
endWorldModelling();
exit(-1);
} else {
printf("Test passed OK!\n");
endWorldModelling();
exit(1);
}
}
}
void checktest9(unsigned long timer)
{
if (timer>500)
{
Vehicle *_b = entities[1];
Vec3f posVector = _b->getPos()-Vec3f(200.0f,1.32f,-6000.0f);
dReal *v = (dReal *)dBodyGetLinearVel(_b->getBodyID());
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
dReal *av = (dReal *)dBodyGetAngularVel(_b->getBodyID());
Vec3f vav;
vav[0]= av[0];vav[1] = av[1]; vav[2] = av[2];
if (posVector.magnitude()>100)
{
printf("Test failed: Walrus is not in their expected position.\n");
endWorldModelling();
exit(-1);
} else if (vav.magnitude()>10)
{
printf("Test failed: Walrus is still moving.\n");
endWorldModelling();
exit(-1);
} else if (vec3fV.magnitude()>5)
{
printf("Test failed: Walrus is still circling.\n");
endWorldModelling();
exit(-1);
} else {
printf("Test passed OK!\n");
endWorldModelling();
exit(1);
}
}
}
SensorReading& Gyroscope::getSensorReading(int localPortId)
{
// Set step counter and check, if new data needs to be computed:
int simulationStep(simulation->getSimulationStep());
if(simulationStep>lastComputationStep)
{
// get the angle velocity of the sensor in world coordinates
ODETools::convertVector(dBodyGetAngularVel(*bodyID), valueAngleVel);
// get the orientation of the gyro sensor
// orientationMatrix contains a 4x3 Matrix
// but only the information from the left 3x3 part is needed to get the orientation of the Obj.
const dReal *orientationMatrix = dBodyGetRotation(*bodyID);
Matrix3d objOrientation;
objOrientation.col[0] = Vector3d(orientationMatrix[0],orientationMatrix[1],orientationMatrix[2]);
objOrientation.col[1] = Vector3d(orientationMatrix[4],orientationMatrix[5],orientationMatrix[6]);
objOrientation.col[2] = Vector3d(orientationMatrix[8],orientationMatrix[9],orientationMatrix[10]);
// project the angle velocity vector which is still in world coordinates to the robot coordinate system
valueAngleVel = objOrientation * valueAngleVel;
lastComputationStep = simulationStep;
}
sensorReading.data.doubleArray = valueAngleVel.v;
return sensorReading;
}
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
dWorldStepFast1 (world,0.05,ITERS);
#endif
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
void CPHDestroyable::NotificatePart(CPHDestroyableNotificate *dn)
{
CPhysicsShell *own_shell=PPhysicsShellHolder()->PPhysicsShell() ;
CPhysicsShell *new_shell=dn->PPhysicsShellHolder()->PPhysicsShell() ;
IKinematics *own_K =smart_cast<IKinematics*>(PPhysicsShellHolder()->Visual());
IKinematics *new_K =smart_cast<IKinematics*>(dn->PPhysicsShellHolder()->Visual()) ;
VERIFY (own_K&&new_K&&own_shell&&new_shell) ;
CInifile *own_ini =own_K->LL_UserData() ;
CInifile *new_ini =new_K->LL_UserData() ;
//////////////////////////////////////////////////////////////////////////////////
Fmatrix own_transform;
own_shell ->GetGlobalTransformDynamic (&own_transform) ;
new_shell ->SetGlTransformDynamic (own_transform) ;
////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////
float random_min =1.f ;
float random_hit_imp =1.f ;
////////////////////////////////////////////////////////////////////////////////////
u16 ref_bone =own_K->LL_GetBoneRoot();
float imp_transition_factor =1.f ;
float lv_transition_factor =1.f ;
float av_transition_factor =1.f ;
////////////////////////////////////////////////////////////////////////////////////
if(own_ini&&own_ini->section_exist("impulse_transition_to_parts"))
{
random_min =own_ini->r_float("impulse_transition_to_parts","random_min");
random_hit_imp =own_ini->r_float("impulse_transition_to_parts","random_hit_imp");
////////////////////////////////////////////////////////
if(own_ini->line_exist("impulse_transition_to_parts","ref_bone"))
ref_bone =own_K->LL_BoneID(own_ini->r_string("impulse_transition_to_parts","ref_bone"));
imp_transition_factor =own_ini->r_float("impulse_transition_to_parts","imp_transition_factor");
lv_transition_factor =own_ini->r_float("impulse_transition_to_parts","lv_transition_factor");
av_transition_factor =own_ini->r_float("impulse_transition_to_parts","av_transition_factor");
if(own_ini->section_exist("collide_parts"))
{
if(own_ini->line_exist("collide_parts","small_object"))
{
new_shell->SetSmall();
}
if(own_ini->line_exist("collide_parts","ignore_small_objects"))
{
new_shell->SetIgnoreSmall();
}
}
}
if(new_ini&&new_ini->section_exist("impulse_transition_from_source_bone"))
{
//random_min =new_ini->r_float("impulse_transition_from_source_bone","random_min");
//random_hit_imp =new_ini->r_float("impulse_transition_from_source_bone","random_hit_imp");
////////////////////////////////////////////////////////
if(new_ini->line_exist("impulse_transition_from_source_bone","ref_bone"))
ref_bone =own_K->LL_BoneID(new_ini->r_string("impulse_transition_from_source_bone","ref_bone"));
imp_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","imp_transition_factor");
lv_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","lv_transition_factor");
av_transition_factor =new_ini->r_float("impulse_transition_from_source_bone","av_transition_factor");
}
//////////////////////////////////////////////////////////////////////////////////////////////////////
dBodyID own_body=own_shell->get_Element(ref_bone)->get_body() ;
u16 new_el_number = new_shell->get_ElementsNumber() ;
for(u16 i=0;i<new_el_number;++i)
{
CPhysicsElement* e=new_shell->get_ElementByStoreOrder(i);
float random_hit=random_min*e->getMass();
if(m_fatal_hit.is_valide() && m_fatal_hit.bone()!=BI_NONE )
{
Fvector pos;
Fmatrix m;m.set(own_K->LL_GetTransform(m_fatal_hit.bone()));
m.mulA_43 (PPhysicsShellHolder()->XFORM());
m.transform_tiny(pos,m_fatal_hit.bone_space_position());
e->applyImpulseVsGF(pos,m_fatal_hit.direction(),m_fatal_hit.phys_impulse()*imp_transition_factor);
random_hit+=random_hit_imp*m_fatal_hit.phys_impulse();
}
Fvector rnd_dir;rnd_dir.random_dir();
e->applyImpulse(rnd_dir,random_hit);
Fvector mc; mc.set(e->mass_Center());
dVector3 res_lvell;
dBodyGetPointVel(own_body,mc.x,mc.y,mc.z,res_lvell);
cast_fv(res_lvell).mul(lv_transition_factor);
e->set_LinearVel(cast_fv(res_lvell));
Fvector res_avell;res_avell.set(cast_fv(dBodyGetAngularVel(own_body)));
res_avell.mul(av_transition_factor);
e->set_AngularVel(res_avell);
}
new_shell->Enable();
new_shell->EnableCollision();
dn->PPhysicsShellHolder()->setVisible(TRUE);
dn->PPhysicsShellHolder()->setEnabled(TRUE);
if(own_shell->IsGroupObject())
new_shell->RegisterToCLGroup(own_shell->GetCLGroup());//CollideBits
CPHSkeleton* ps=dn->PPhysicsShellHolder()->PHSkeleton();
if(ps)
{
if(own_ini&&own_ini->section_exist("autoremove_parts"))
{
ps->SetAutoRemove(1000*(READ_IF_EXISTS(own_ini,r_u32,"autoremove_parts","time",ps->DefaultExitenceTime())));
}
if(new_ini&&new_ini->section_exist("autoremove"))
{
ps->SetAutoRemove(1000*(READ_IF_EXISTS(new_ini,r_u32,"autoremove","time",ps->DefaultExitenceTime())));
}
}
}
void dampRotationalMotion (dReal kd)
{
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]);
}
void CProtoHapticDoc::SimulationStep()
{
int elapsed= clock() - m_lastSimStep;
int steps= (int)((((float)elapsed)*m_simSpeed)*0.1f);
if(steps<1) {
m_lastSimStep= clock();
return;
}
// collision detection
for(int i= 0; i<m_shapeCount; i++) {
if(m_shapes[i]->isCollisionDynamic())
dGeomSetBody (m_geoms[i], bodies[i]);
else
dGeomSetBody (m_geoms[i], 0);
}
dSpaceCollide (m_spaceID,this,&nearCallbackStatic);
dWorldQuickStep (m_worldID, steps);
dJointGroupEmpty (m_jointGroup);
for( int i= 0; i<m_shapeCount; i++) {
//air resistance
if(m_shapes[i]->isCollisionDynamic()||m_shapes[i]->isProxyDynamic()) {
const dReal *vel;
const dReal *angvel;
vel= dBodyGetLinearVel (bodies[i]);
dBodyAddForce (bodies[i], -vel[0]*m_airResistance, -vel[1]*m_airResistance, -vel[2]*m_airResistance);
angvel= dBodyGetAngularVel (bodies[i]);
dBodyAddTorque (bodies[i], -angvel[0]*m_airResistance, -angvel[1]*m_airResistance, -angvel[2]*m_airResistance);
}
HHLRC rc= hlGetCurrentContext();
// proxy0
hlMakeCurrent(m_context);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
if(((CProtoHapticApp*)AfxGetApp())->isTwoDevices()) {
// proxy1
hlMakeCurrent(m_context_1);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching1()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
}
hlMakeCurrent(rc);
// gravity
if(m_shapes[i]->isCollisionDynamic())
dBodyAddForce(bodies[i], 0.0, -m_shapes[i]->getMass()*(m_gravity/10.0f), 0.0);
const dReal *pos;
const dReal *rot;
pos= dBodyGetPosition (bodies[i]);
rot= dBodyGetRotation (bodies[i]);
float rotation[16];
rotation[0]= rot[0]; rotation[4]= rot[1]; rotation[8]= rot[2]; rotation[12]= rot[3];
rotation[1]= rot[4]; rotation[5]= rot[5]; rotation[9]= rot[6]; rotation[13]= rot[7];
rotation[2]= rot[8]; rotation[6]= rot[9]; rotation[10]= rot[10]; rotation[14]= rot[11];
rotation[3]= 0.0; rotation[7]= 0.0; rotation[11]= 0.0; rotation[15]= 1.0;
m_shapes[i]->setRotation(rotation);
m_shapes[i]->setLocation(pos[0], pos[1], pos[2]);
}
m_lastSimStep= clock();
}
void
dxJointTransmission::getInfo2( dReal worldFPS,
dReal /*worldERP*/,
const Info2Descr* info )
{
dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
const dReal *p[2], *omega[2];
int i;
// Transform all needed quantities to the global frame.
for (i = 0 ; i < 2 ; i += 1) {
dBodyGetRelPointPos(node[i].body,
anchors[i][0], anchors[i][1], anchors[i][2],
a[i]);
dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
n[i]);
p[i] = dBodyGetPosition(node[i].body);
omega[i] = dBodyGetAngularVel(node[i].body);
}
if (update) {
// Make sure both gear reference frames end up with the same
// handedness.
if (dCalcVectorDot3(n[0], n[1]) < 0) {
dNegateVector3(axes[0]);
dNegateVector3(n[0]);
}
}
// Calculate the mesh geometry based on the current mode.
switch (mode) {
case dTransmissionParallelAxes:
// Simply calculate the contact point as the point on the
// baseline that will yield the correct ratio.
dIASSERT (ratio > 0);
dSubtractVectors3(d, a[1], a[0]);
dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
dCopyVector3(c[1], c[0]);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
dCalcVectorCross3(l[i], d, n[i]);
}
break;
case dTransmissionIntersectingAxes:
// Calculate the line of intersection between the planes of the
// gears.
dCalcVectorCross3(l[0], n[0], n[1]);
dCopyVector3(l[1], l[0]);
nn = dCalcVectorDot3(n[0], n[1]);
dIASSERT(fabs(nn) != 1);
na_0 = dCalcVectorDot3(n[0], a[0]);
na_1 = dCalcVectorDot3(n[1], a[1]);
dAddScaledVectors3(O, n[0], n[1],
(na_0 - na_1 * nn) / (1 - nn * nn),
(na_1 - na_0 * nn) / (1 - nn * nn));
// Find the contact point as:
//
// c = ((r_a - O) . l) l + O
//
// where r_a the anchor point of either gear and l, O the tangent
// line direction and origin.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3(d, a[i], O);
m = dCalcVectorDot3(d, l[i]);
dAddScaledVectors3(c[i], O, l[i], 1, m);
}
break;
case dTransmissionChainDrive:
dSubtractVectors3(d, a[0], a[1]);
m = dCalcVectorLength3(d);
dIASSERT(m > 0);
// Caclulate the angle of the contact point relative to the
// baseline.
cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);
dNormalize3(d);
for (i = 0 ; i < 2 ; i += 1) {
// Calculate the contact radius in the local reference
// frame of the chain. This has axis x pointing along the
// baseline, axis y pointing along the sprocket axis and
// the remaining axis normal to both.
u[0] = radii[i] * cosphi;
u[1] = 0;
u[2] = radii[i] * sinphi;
// Transform the contact radius into the global frame.
dCalcVectorCross3(z, d, n[i]);
v[0] = dCalcVectorDot3(d, u);
v[1] = dCalcVectorDot3(n[i], u);
v[2] = dCalcVectorDot3(z, u);
// Finally calculate contact points and l.
dAddVectors3(c[i], a[i], v);
dCalcVectorCross3(l[i], v, n[i]);
dNormalize3(l[i]);
// printf ("%d: %f, %f, %f\n",
// i, l[i][0], l[i][1], l[i][2]);
}
break;
}
if (update) {
// We need to calculate an initial reference frame for each
// wheel which we can measure the current phase against. This
// frame will have the initial contact radius as the x axis,
// the wheel axis as the z axis and their cross product as the
// y axis.
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], a[i]);
radii[i] = dCalcVectorLength3(r[i]);
dIASSERT(radii[i] > 0);
dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
reference[i]);
dNormalize3(reference[i]);
dCopyVector3(reference[i] + 8, axes[i]);
dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);
// printf ("%f\n", dDOT(r[i], n[i]));
// printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
// reference[i][0],reference[i][1],reference[i][2],
// reference[i][4],reference[i][5],reference[i][6],
// reference[i][8],reference[i][9],reference[i][10]);
radii[i] = radii[i];
phase[i] = 0;
}
ratio = radii[0] / radii[1];
update = 0;
}
for (i = 0 ; i < 2 ; i += 1) {
dReal phase_hat;
dSubtractVectors3 (r[i], c[i], a[i]);
// Transform the (global) contact radius into the gear's
// reference frame.
dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
dMultiply0_331(t, reference[i], s);
// Now simply calculate its angle on the plane relative to the
// x-axis which is the initial contact radius. This will be
// an angle between -pi and pi that is coterminal with the
// actual phase of the wheel. To find the real phase we
// estimate it by adding omega * dt to the old phase and then
// find the closest angle to that, that is coterminal to
// theta.
theta = atan2(t[1], t[0]);
phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;
if (phase_hat > M_PI_2) {
if (theta < 0) {
theta += (dReal)(2 * M_PI);
}
theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
} else if (phase_hat < -M_PI_2) {
if (theta > 0) {
theta -= (dReal)(2 * M_PI);
}
theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
}
if (phase_hat - theta > M_PI) {
phase[i] = theta + (dReal)(2 * M_PI);
} else if (phase_hat - theta < -M_PI) {
phase[i] = theta - (dReal)(2 * M_PI);
} else {
phase[i] = theta;
}
dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
}
// Calculate the phase error. Depending on the mode the condition
// is that the distances traveled by each contact point must be
// either equal (chain and sprockets) or opposite (gears).
if (mode == dTransmissionChainDrive) {
delta = (dCalcVectorLength3(r[0]) * phase[0] -
dCalcVectorLength3(r[1]) * phase[1]);
} else {
delta = (dCalcVectorLength3(r[0]) * phase[0] +
dCalcVectorLength3(r[1]) * phase[1]);
}
// When in chain mode a torque reversal, signified by the change
// in sign of the wheel phase difference, has the added effect of
// switching the active chain branch. We must therefore reflect
// the contact points and tangents across the baseline.
if (mode == dTransmissionChainDrive && delta < 0) {
dVector3 d;
dSubtractVectors3(d, a[0], a[1]);
for (i = 0 ; i < 2 ; i += 1) {
dVector3 nn;
dReal a;
dCalcVectorCross3(nn, n[i], d);
a = dCalcVectorDot3(nn, nn);
dIASSERT(a > 0);
dAddScaledVectors3(c[i], c[i], nn,
1, -2 * dCalcVectorDot3(c[i], nn) / a);
dAddScaledVectors3(l[i], l[i], nn,
-1, 2 * dCalcVectorDot3(l[i], nn) / a);
}
}
// Do not add the constraint if there's backlash and we're in the
// backlash gap.
if (backlash == 0 || fabs(delta) > backlash) {
// The constraint is satisfied iff the absolute velocity of the
// contact point projected onto the tangent of the wheels is equal
// for both gears. This velocity can be calculated as:
//
// u = v + omega x r_c
//
// The constraint therefore becomes:
// (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
// (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2
for (i = 0 ; i < 2 ; i += 1) {
dSubtractVectors3 (r[i], c[i], p[i]);
}
dCalcVectorCross3(info->J1a, r[0], l[0]);
dCalcVectorCross3(info->J2a, l[1], r[1]);
dCopyVector3(info->J1l, l[0]);
dCopyNegatedVector3(info->J2l, l[1]);
if (delta > 0) {
if (backlash > 0) {
info->lo[0] = -dInfinity;
info->hi[0] = 0;
}
info->c[0] = -worldFPS * erp * (delta - backlash);
} else {
if (backlash > 0) {
info->lo[0] = 0;
info->hi[0] = dInfinity;
}
info->c[0] = -worldFPS * erp * (delta + backlash);
}
}
info->cfm[0] = cfm;
// printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);
// Cache the contact point (in world coordinates) to avoid
// recalculation if requested by the user.
dCopyVector3(contacts[0], c[0]);
dCopyVector3(contacts[1], c[1]);
}
const dReal* ODE_Particle::getAngularVel()
{
return dBodyGetAngularVel(body);
}
/* returns the angular velocity of body b in the body reference frame */
void body_angular_velocity_principal_axes (t_real *res, dBodyID b) {
const t_real * omega = dBodyGetAngularVel (b);
dBodyVectorFromWorld (b, omega[0], omega[1], omega[2], res);
}
void Cylinder::applySimpleRollingFriction(double rfc)
{
const dReal* curAngVel = dBodyGetAngularVel(body);
dBodyAddTorque(body, (curAngVel[0] * (dReal)(-rfc)), (curAngVel[1] * (dReal)(-rfc)), (curAngVel[2] * (dReal)(-rfc)));
}
bool SimObjectRenderer::moveDrag(int x, int y)
{
if(!dragging)
return false;
if(!dragSelection) // camera control
{
if(cameraMode == SimRobotCore2::Renderer::targetCam)
{
if(dragType == dragRotate || dragType == dragRotateWorld)
{
}
else // if(dragType == dragNormal)
rotateCamera((x - dragStartPos.x) * -0.01f, (y - dragStartPos.y) * -0.01f);
}
else // if(cameraMode == SimRobotCore2::Renderer::freeCam)
{
if(dragType == dragRotate || dragType == dragRotateWorld)
{
}
else // if(dragType == dragNormal)
rotateCamera((x - dragStartPos.x) * -0.001f, (y - dragStartPos.y) * -0.001f);
}
dragStartPos.x = x;
dragStartPos.y = y;
return true;
}
else // object control
{
if(dragMode == applyDynamics)
return true;
Vector3<> projectedClick = projectClick(x, y);
Vector3<> currentPos;
if(intersectRayAndPlane(cameraPos, projectedClick - cameraPos, dragSelection->pose.translation, dragPlaneVector, currentPos))
{
if(dragType == dragRotate || dragType == dragRotateWorld)
{
Vector3<> oldv = dragStartPos - dragSelection->pose.translation;
Vector3<> newv = currentPos - dragSelection->pose.translation;
if(dragType != dragRotateWorld)
{
Matrix3x3<> invRotation = dragSelection->pose.rotation.transpose();
oldv = invRotation * oldv;
newv = invRotation * newv;
}
float angle = 0.f;
if(dragPlane == yzPlane)
angle = normalize(atan2f(newv.z, newv.y) - atan2f(oldv.z, oldv.y));
else if(dragPlane == xzPlane)
angle = normalize(atan2f(newv.x, newv.z) - atan2f(oldv.x, oldv.z));
else
angle = normalize(atan2f(newv.y, newv.x) - atan2f(oldv.y, oldv.x));
Vector3<> offset = dragPlaneVector * angle;
Matrix3x3<> rotation(offset);
Vector3<> center = dragSelection->pose.translation;
dragSelection->rotate(rotation, center);
if(dragMode == adoptDynamics)
{
unsigned int now = System::getTime();
float t = (now - dragStartTime) * 0.001f;
Vector3<> velocity = offset / t;
const dReal* oldVel = dBodyGetAngularVel(dragSelection->body);
velocity = velocity * 0.3f + Vector3<>((float) oldVel[0], (float) oldVel[1], (float) oldVel[2]) * 0.7f;
dBodySetAngularVel(dragSelection->body, velocity.x, velocity.y, velocity.z);
dragStartTime = now;
}
dragStartPos = currentPos;
}
else
{
const Vector3<> offset = currentPos - dragStartPos;
dragSelection->move(offset);
if(dragMode == adoptDynamics)
{
unsigned int now = System::getTime();
float t = (now - dragStartTime) * 0.001f;
Vector3<> velocity = offset / t;
const dReal* oldVel = dBodyGetLinearVel(dragSelection->body);
velocity = velocity * 0.3f + Vector3<>((float) oldVel[0], (float) oldVel[1], (float) oldVel[2]) * 0.7f;
dBodySetLinearVel(dragSelection->body, velocity.x, velocity.y, velocity.z);
dragStartTime = now;
}
dragStartPos = currentPos;
}
}
return true;
}
}
/* returns the angular velocity of body b in the lab frame reference */
void body_angular_velocity (t_real *res, dBodyID b) {
dCopyVector3 (res, dBodyGetAngularVel (b));
}
void ODERigidObject::GetVelocity(Vector3& w,Vector3& v) const
{
CopyVector(v,dBodyGetLinearVel(bodyID));
CopyVector(w,dBodyGetAngularVel(bodyID));
}
void ODESimulator::stepPhysics()
{
// Apply linear and angular damping; if using the "add opposing
// forces" method, be sure to do this before calling ODE step
// function.
std::vector<Solid*>::iterator iter;
for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter)
{
ODESolid* solid = (ODESolid*) (*iter);
if (!solid->isStatic())
{
if (solid->isSleeping())
{
// Reset velocities, force, & torques of objects that go
// to sleep; ideally, this should happen in the ODE
// source only once - when the object initially goes to
// sleep.
dBodyID bodyID = ((ODESolid*) solid)->internal_getBodyID();
dBodySetLinearVel(bodyID, 0, 0, 0);
dBodySetAngularVel(bodyID, 0, 0, 0);
dBodySetForce(bodyID, 0, 0, 0);
dBodySetTorque(bodyID, 0, 0, 0);
}
else
{
// Dampen Solid motion. 3 possible methods:
// 1) apply a force/torque in the opposite direction of
// motion scaled by the body's velocity
// 2) same as 1, but scale the opposing force by
// the body's momentum (this automatically handles
// different mass values)
// 3) reduce the body's linear and angular velocity by
// scaling them by 1 - damping * stepsize
dBodyID bodyID = solid->internal_getBodyID();
dMass mass;
dBodyGetMass(bodyID, &mass);
// Method 1
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = -solid->getLinearDamping();
//dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* a = dBodyGetAngularVel(bodyID);
//damping = -solid->getAngularDamping();
//dBodyAddTorque(bodyID, damping*a[0], damping*a[1], damping*a[2]);
// Method 2
// Since the ODE mass.I inertia matrix is local, angular
// velocity and torque also need to be local.
// Linear damping
real linearDamping = solid->getLinearDamping();
if (0 != linearDamping)
{
const dReal * lVelGlobal = dBodyGetLinearVel(bodyID);
// The damping force depends on the damping amount,
// mass, and velocity (i.e. damping amount and
// momentum).
dReal dampingFactor = -linearDamping * mass.mass;
dVector3 dampingForce = {
dampingFactor * lVelGlobal[0],
dampingFactor * lVelGlobal[1],
dampingFactor * lVelGlobal[2] };
// Add a global force opposite to the global linear
// velocity.
dBodyAddForce(bodyID, dampingForce[0],
dampingForce[1], dampingForce[2]);
}
// Angular damping
real angularDamping = solid->getAngularDamping();
if (0 != angularDamping)
{
const dReal * aVelGlobal = dBodyGetAngularVel(bodyID);
dVector3 aVelLocal;
dBodyVectorFromWorld(bodyID, aVelGlobal[0],
aVelGlobal[1], aVelGlobal[2], aVelLocal);
// The damping force depends on the damping amount,
// mass, and velocity (i.e. damping amount and
// momentum).
//dReal dampingFactor = -angularDamping * mass.mass;
dReal dampingFactor = -angularDamping;
dVector3 aMomentum;
// Make adjustments for inertia tensor.
dMULTIPLYOP0_331(aMomentum, = , mass.I, aVelLocal);
dVector3 dampingTorque = {
dampingFactor * aMomentum[0],
dampingFactor * aMomentum[1],
dampingFactor * aMomentum[2] };
// Add a local torque opposite to the local angular
// velocity.
dBodyAddRelTorque(bodyID, dampingTorque[0],
dampingTorque[1], dampingTorque[2]);
}
//dMass mass;
//dBodyGetMass(bodyID, &mass);
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = -solid->getLinearDamping() * mass.mass;
//dBodyAddForce(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* aVelLocal = dBodyGetAngularVel(bodyID);
////dVector3 aVelLocal;
////dBodyVectorFromWorld(bodyID, aVelGlobal[0], aVelGlobal[1], aVelGlobal[2], aVelLocal);
//damping = -solid->getAngularDamping();
//dVector3 aMomentum;
//dMULTIPLYOP0_331(aMomentum, =, aVelLocal, mass.I);
//dBodyAddTorque(bodyID, damping*aMomentum[0], damping*aMomentum[1],
// damping*aMomentum[2]);
// Method 3
//const dReal* l = dBodyGetLinearVel(bodyID);
//dReal damping = (real)1.0 - solid->getLinearDamping() * mStepSize;
//dBodySetLinearVel(bodyID, damping*l[0], damping*l[1], damping*l[2]);
//const dReal* a = dBodyGetAngularVel(bodyID);
//damping = (real)1.0 - solid->getAngularDamping() * mStepSize;
//dBodySetAngularVel(bodyID, damping*a[0], damping*a[1], damping*a[2]);
}
}
}
// Do collision detection; add contacts to the contact joint group.
dSpaceCollide(mRootSpaceID, this,
&ode_hidden::internal_collisionCallback);
// Take a simulation step.
if (SOLVER_QUICKSTEP == mSolverType)
{
dWorldQuickStep(mWorldID, mStepSize);
}
else
{
dWorldStep(mWorldID, mStepSize);
}
// Remove all joints from the contact group.
dJointGroupEmpty(mContactJointGroupID);
// Fix angular velocities for freely-spinning bodies that have
// gained angular velocity through explicit integrator inaccuracy.
for (iter = mSolidList.begin(); iter != mSolidList.end(); ++iter)
{
ODESolid* s = (ODESolid*) (*iter);
if (!s->isSleeping() && !s->isStatic())
{
s->internal_doAngularVelFix();
}
}
}
