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(); } } }