void OscFreeODE::simulationCallback() { ODEConstraint& me = *static_cast<ODEConstraint*>(special()); const dReal *pos1 = dBodyGetPosition(me.body1()); const dReal *pos2 = dBodyGetPosition(me.body2()); const dReal *vel1 = dBodyGetLinearVel(me.body1()); const dReal *vel2 = dBodyGetLinearVel(me.body2()); dReal force[3]; force[0] = - ((pos2[0]-pos1[0]-m_initial_distance[0]) * m_response->m_stiffness.m_value) - (vel2[0]-vel1[0]) * m_response->m_damping.m_value; force[1] = - ((pos2[1]-pos1[1]-m_initial_distance[1]) * m_response->m_stiffness.m_value) - (vel2[1]-vel1[1]) * m_response->m_damping.m_value; force[2] = - ((pos2[2]-pos1[2]-m_initial_distance[2]) * m_response->m_stiffness.m_value) - (vel2[2]-vel1[2]) * m_response->m_damping.m_value; dBodyAddForce(me.body1(), -force[0], -force[1], -force[2]); dBodyAddForce(me.body2(), force[0], force[1], force[2]); }
int main(int argc, char** argv) { init_gfx(); init_2d(); world = dWorldCreate(); dBodyID ball = dBodyCreate(world); dBodySetPosition(ball, 0, 0, 0); for (;;) { Uint8* chars = SDL_GetKeyState(NULL); if (chars[SDLK_LEFT]) { dBodyAddForce(ball, -force, 0, 0); } if (chars[SDLK_RIGHT]) { dBodyAddForce(ball, +force, 0, 0); } if (chars[SDLK_UP]) { dBodyAddForce(ball, 0, +force, 0); } if (chars[SDLK_DOWN]) { dBodyAddForce(ball, 0, -force, 0); } const dReal* pos = dBodyGetPosition(ball); glTranslatef(pos[0], pos[1], pos[2]); draw_circle(); dWorldQuickStep(world, 0.01); step(); } }
void addSpringForce (dReal ks) { 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])); }
void Body::stepPhysics () { WorldObject::stepPhysics(); dBodyID bodyID = getMainOdeObject()->getBodyID(); if (userDriver) { double moveZ = System::get()->axisMap[getIDKeyboardKey(SDLK_BACKSPACE)]->getValue() * 50000; moveZ += System::get()->axisMap[getIDKeyboardKey(SDLK_RETURN)]->getValue() * 12200; moveZ -= System::get()->axisMap[getIDKeyboardKey(SDLK_RSHIFT)]->getValue() * 10000; dBodyAddForce (bodyID, 0, 0, moveZ); } // apply simple air drag forces: const double airDensity = 1.225; // f = Cx * 0.5 * airDensity * v^2 * area; // f = dragCoefficient * 0.5 * 1.225 * vel*vel * frontalArea; Vector3d velocity (dBodyGetLinearVel (bodyID)); double velModule = velocity.distance(); Vector3d normalizedVel (velocity); normalizedVel.scalarDivide(velModule); normalizedVel.scalarMultiply(-1); Vector3d force (normalizedVel); force.scalarMultiply (0.5 * dragCoefficient * airDensity * frontalArea * velModule * velModule); dBodyAddForce (bodyID, force.x, force.y, force.z); //log->__format(LOG_DEVELOPER, "Body air drag force = (%f, %f, %f)", force.x, force.y, force.z); }
void controller::virtualForces() { //For front leg frame if(swingFlag[0] == false || swingFlag[1] == false) { float force = lf_velocity_force_kv[0]*(desired_velocity[0] - current_velocity[0]); dBodyAddForce(body_bag->getBackLink1Body(), force, 0.0, 0.0); } if(swingFlag[2] == false || swingFlag[3] == false) { float force = lf_velocity_force_kv[1]*(desired_velocity[0] - current_velocity[0]); dBodyAddForce(body_bag->getBackLink6Body(), force, 0.0, 0.0); } }
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); }
// start simulation - set viewpoint (camera) static void start() { // Original View static float xyz[3] = {3.0f, -3.0f, 8.0f}; static float hpr[3] = {136.000f,-50.0000f,0.0000f}; // Top Down View // static float xyz[3] = {0.0f,-8.0f,1.0f}; // static float hpr[3] = {90.0f,0.0f,0.0f}; dBodyAddForce(sphere0,50,0,0); dBodyAddForce(sphere1,-300,0,0); dBodyAddForce(sphere2,-10,0,0); dsSetViewpoint (xyz,hpr); }
/*! this is the reimplementation of the virtual function of OpenDEEnvironment * that explain how the control will apply depending on the action type such as pull, push or move. * Here the controls are applying by simply applying the force. */ void twoDRobotEnvironment::applyControl (const double *control) const { if(manipulationQuery->getActionType()=="move" || manipulationQuery->getActionType()=="Move") { dBodyAddForce(bodies[0],control[0],control[1],0.0); } else if(manipulationQuery->getActionType()=="pull" || manipulationQuery->getActionType()=="Pull" || manipulationQuery->getActionType()=="push" || manipulationQuery->getActionType()=="Push") { dBodyAddForce(bodies[0],manipulationQuery->getforce().at(0),manipulationQuery->getforce().at(1),manipulationQuery->getforce().at(2)); } else { std::cout<<"Invalid Action"<<std::endl;} }
void addOscillatingForce (dReal fscale) { static dReal a=0; dBodyAddForce (body[0],fscale*cos(2*a),fscale*cos(2.7183*a), fscale*cos(1.5708*a)); a += 0.01; }
static void simLoop (int pause) { if (!pause) { // add random forces and torques to all bodies int i; const dReal scale1 = 5; const dReal scale2 = 5; for (i=0; i<NUM; i++) { dBodyAddForce (body[i], scale1*(dRandReal()*2-1), scale1*(dRandReal()*2-1), scale1*(dRandReal()*2-1)); dBodyAddTorque (body[i], scale2*(dRandReal()*2-1), scale2*(dRandReal()*2-1), scale2*(dRandReal()*2-1)); } dWorldStep (world,0.05); createTest(); } // float sides[3] = {SIDE,SIDE,SIDE}; dsSetColor (1,1,0); for (int i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS); }
bool Primitive::applyForce(double x, double y, double z){ if(body){ dBodyAddForce(body, x, y, z); return true; } else return false; }
bool SimObjectRenderer::releaseDrag(int x, int y) { if(!dragging) return false; if(!dragSelection) // camera control { dragging = false; return true; } else // object control { if(dragMode == adoptDynamics) moveDrag(x, y); else if(dragMode == applyDynamics) { 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; Vector3<> torque = offset * (float) dragSelection->mass.mass * 50.f; dBodyAddTorque(dragSelection->body, torque.x, torque.y, torque.z); } else { const Vector3<> offset = currentPos - dragStartPos; Vector3<> force = offset * (float) dragSelection->mass.mass * 500.f; dBodyAddForce(dragSelection->body, force.x, force.y, force.z); } } } dragSelection->enablePhysics(true); dragging = false; return true; } }
void add_phys_force(dBodyID body, float x, float y, float z) { if (body) { dBodyEnable(body); dBodyAddForce(body, x, y, z); } }
void GLWidget::keyPressEvent(QKeyEvent *event) { if (event->key() == Qt::Key_Control) ctrl = true; if (event->key() == Qt::Key_Alt) alt = true; char cmd = event->key(); if (fullScreen) { if (event->key()==Qt::Key_F2) emit toggleFullScreen(false); } const dReal S = 0.30; const dReal BallForce = 0.2; int R = robotIndex(Current_robot,Current_team); switch (cmd) { case 't': case 'T': ssl->robots[R]->incSpeed(0,-S);ssl->robots[R]->incSpeed(1,S);ssl->robots[R]->incSpeed(2,-S);ssl->robots[R]->incSpeed(3,S);break; case 'g': case 'G': ssl->robots[R]->incSpeed(0,S);ssl->robots[R]->incSpeed(1,-S);ssl->robots[R]->incSpeed(2,S);ssl->robots[R]->incSpeed(3,-S);break; case 'f': case 'F': ssl->robots[R]->incSpeed(0,S);ssl->robots[R]->incSpeed(1,S);ssl->robots[R]->incSpeed(2,S);ssl->robots[R]->incSpeed(3,S);break; case 'h': case 'H': ssl->robots[R]->incSpeed(0,-S);ssl->robots[R]->incSpeed(1,-S);ssl->robots[R]->incSpeed(2,-S);ssl->robots[R]->incSpeed(3,-S);break; case 'w': case 'W':dBodyAddForce(ssl->ball->body,0, BallForce,0);break; case 's': case 'S':dBodyAddForce(ssl->ball->body,0,-BallForce,0);break; case 'd': case 'D':dBodyAddForce(ssl->ball->body, BallForce,0,0);break; case 'a': case 'A':dBodyAddForce(ssl->ball->body,-BallForce,0,0);break; case 'k':case 'K': ssl->robots[R]->kicker->kick(4,0);break; case 'l':case 'L': ssl->robots[R]->kicker->kick(2,2);break; case 'j':case 'J': ssl->robots[R]->kicker->toggleRoller();break; case 'i':case 'I': dBodySetLinearVel(ssl->ball->body,2.0,0,0);dBodySetAngularVel(ssl->ball->body,0,2.0/cfg->v_BallRadius->getValue(),0);break; case ';': if (kickingball==false) { kickingball = true; logStatus(QString("Kick mode On"),QColor("blue")); } else { kickingball = false; logStatus(QString("Kick mode Off"),QColor("red")); } break; case ']': kickpower += 0.1; logStatus(QString("Kick power = %1").arg(kickpower),QColor("orange"));break; case '[': kickpower -= 0.1; logStatus(QString("Kick power = %1").arg(kickpower),QColor("cyan"));break; case ' ': ssl->robots[R]->resetSpeeds(); break; case '`': dBodySetLinearVel(ssl->ball->body,0,0,0); dBodySetAngularVel(ssl->ball->body,0,0,0); break; } }
void TTestDepthCallback (bool& do_colide,bool bo1,dContact& c,SGameMtl* material_1,SGameMtl* material_2) { if(saved_callback)saved_callback(do_colide,bo1,c,material_1,material_2); if(do_colide&&!material_1->Flags.test(SGameMtl::flPassable) &&!material_2->Flags.test(SGameMtl::flPassable)) { float& depth=c.geom.depth; float test_depth=depth-Pars::decrement_depth; save_max(max_depth,test_depth); c.surface.mu*=Pars::calback_friction_factor; if(test_depth>Pars::depth_to_use_force) { float force = Pars::callback_force_factor*ph_world->Gravity(); dBodyID b1=dGeomGetBody(c.geom.g1); dBodyID b2=dGeomGetBody(c.geom.g2); if(b1)dBodyAddForce(b1,c.geom.normal[0]*force,c.geom.normal[1]*force,c.geom.normal[2]*force); if(b2)dBodyAddForce(b2,-c.geom.normal[0]*force,-c.geom.normal[1]*force,-c.geom.normal[2]*force); dxGeomUserData* ud1=retrieveGeomUserData(c.geom.g1); dxGeomUserData* ud2=retrieveGeomUserData(c.geom.g2); if(ud1) { CPhysicsShell* phsl=ud1->ph_ref_object->PPhysicsShell(); if(phsl) phsl->Enable(); } if(ud2) { CPhysicsShell* phsl=ud2->ph_ref_object->PPhysicsShell(); if(phsl) phsl->Enable(); } do_colide=false; } else if(test_depth>Pars::depth_to_change_softness_pars) { c.surface.soft_cfm=Pars::callback_cfm_factor; c.surface.soft_erp=Pars::callback_erp_factor; } limit_above(depth,Pars::max_real_depth); } }
IoObject *IoODEBody_addForce(IoODEBody *self, IoObject *locals, IoMessage *m) { const double x = IoMessage_locals_doubleArgAt_(m, locals, 0); const double y = IoMessage_locals_doubleArgAt_(m, locals, 1); const double z = IoMessage_locals_doubleArgAt_(m, locals, 2); IoODEBody_assertValidBody(self, locals, m); dBodyAddForce(BODYID, x, y, z); return self; }
void CBall::AddForce(dReal dt) { dReal coef = 0.015f; const dReal* vel; vel = dBodyGetLinearVel(bBall); dBodyAddForce( bBall, -vel[0]*coef, -vel[1]*coef, -vel[2]*coef ); }
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]); }
void dJointAddScrewForce ( dJointID j, dReal force ) { dxJointScrew* joint = ( dxJointScrew* ) j; dVector3 axis; dUASSERT ( joint, "bad joint argument" ); checktype ( joint, Screw ); if ( joint->flags & dJOINT_REVERSE ) force -= force; getAxis ( joint, axis, joint->axis1 ); axis[0] *= force; axis[1] *= force; axis[2] *= force; if ( joint->node[0].body != 0 ) dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] ); if ( joint->node[1].body != 0 ) dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] ); if ( joint->node[0].body != 0 && joint->node[1].body != 0 ) { // linear torque decoupling: // we have to compensate the torque, that this screw force may generate // if body centers are not aligned along the screw axis dVector3 ltd; // Linear Torque Decoupling vector (a torque) dVector3 cgdiff; cgdiff[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] ); cgdiff[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] ); cgdiff[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] ); dCalcVectorCross3( ltd, cgdiff, axis ); dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] ); dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] ); } }
int main(int argc, char** argv) { init_gfx(); init_2d(); world = dWorldCreate(); dBodyID ball = dBodyCreate(world); dBodySetPosition(ball, 0, 0, 0); for (;;) { Uint8* chars = SDL_GetKeyState(NULL); const dReal* pos = dBodyGetPosition(ball); const dReal* q = dBodyGetQuaternion(ball); double angle = get_q_angle(q); const dReal* axis = get_q_axis(q); if (chars[SDLK_LEFT]) { dBodyAddForceAtPos(ball, -force, 0, 0, pos[0], pos[1]+1, pos[2]); } if (chars[SDLK_RIGHT]) { dBodyAddForceAtPos(ball, force, 0, 0, pos[0], pos[1]+1, pos[2]); } if (chars[SDLK_UP]) { dBodyAddForce(ball, 0, +force, 0); } if (chars[SDLK_DOWN]) { dBodyAddForce(ball, 0, -force, 0); } glTranslatef(pos[0], pos[1], pos[2]); glRotatef(angle * 180 / M_PI, axis[0], axis[1], axis[2]); draw_circle(); dWorldQuickStep(world, 0.01); step(); } }
void ODE_ForceHandle::updateActuators() { if(mHostBody != 0 && mActivateForces->get()) { dBodyID hostBodyId = mHostBody->getRigidBodyID(); if(hostBodyId != 0) { if(mApplyRelativeForces->get()) { dBodyAddForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ()); dBodyAddTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ()); } else { dBodyAddRelForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ()); dBodyAddRelTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ()); } } } }
void base::move(int size, int max) { //for wall-up i must be a sphere with total-mass, torq in air and in stand, and with force in air if(body) dBodyGetRelPointVel(body,0,0,0,pointrelvel); // const dReal *tr = dBodyGetTorque(body); if (up){ //in up if (size > 0 && pointrelvel[0]<max) dBodyAddForce (body,size/10,0,0); // if (size > 0 && pointrelvel[0]<max) dBodyAddTorque (body,size/4, 0, 0); else if (size < 0 && pointrelvel[0]>-max) dBodyAddForce (body,size/10,0,0); // else if (size < 0 && pointrelvel[0]>-max) dBodyAddTorque (body,size,0,0); } else{ if (size > 0 && pointrelvel[0] < max) { dBodyAddForce (body,size,0,0); // dBodyAddRelTorque (body, 0, 0, -size*60); } else if (size < 0 && pointrelvel[0] > -max) { dBodyAddForce (body,size,0,0); // dBodyAddTorque (body, 0, 0, -size*60); } //~ if (up) //in up //~ { //~ if (size > 0 && pointrelvel[0]<max) dBodyAddForce (body,size/4,0,0); //~ else if (size < 0 && pointrelvel[0]>-max) dBodyAddForce (body,size/4,0,0); //~ } //~ //~ else //~ { //~ if (size > 0 && pointrelvel[0] < max) dBodyAddForce (body, size,0,0); //~ else if (size < 0 && pointrelvel[0] > -max) dBodyAddForce (body,size,0,0); //~ } }
void PhysicsSim::step() { // Add extra forces to objects // Grabbed object attraction if (m_pGrabbedObject) { cVector3d grab_force(m_pGrabbedObject->getPosition() - m_pCursor->m_position); grab_force.mul(-0.01); grab_force.add(m_pGrabbedODEObject->getVelocity()*(-0.0003)); dBodyAddForce(m_pGrabbedODEObject->body(), grab_force.x, grab_force.y, grab_force.z); } // Perform simulation step dSpaceCollide (m_odeSpace, this, &ode_nearCallback); dWorldStepFast1 (m_odeWorld, m_fTimestep, 5); dJointGroupEmpty (m_odeContactGroup); /* Update positions of each object in the other simulations */ std::map<std::string,OscObject*>::iterator it; for (it=world_objects.begin(); it!=world_objects.end(); it++) { ODEObject *o = static_cast<ODEObject*>(it->second->special()); if (o) { o->update(); cVector3d pos(o->getPosition()); cMatrix3d rot(o->getRotation()); send(true, (it->second->path()+"/position").c_str(), "fff", pos.x,pos.y,pos.z); send(true, (it->second->path()+"/rotation").c_str(), "fffffffff", rot.m[0][0], rot.m[0][1], rot.m[0][2], rot.m[1][0], rot.m[1][1], rot.m[1][2], rot.m[2][0], rot.m[2][1], rot.m[2][2]); } } /* Update the responses of each constraint. */ std::map<std::string,OscConstraint*>::iterator cit; for (cit=world_constraints.begin(); cit!=world_constraints.end(); cit++) { cit->second->simulationCallback(); } m_counter++; }
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] ); }
static void simLoop (int pause) { int i; if (!pause) { static double angle = 0; angle += 0.05; dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0)); dSpaceCollide (space,0,&nearCallback); dWorldStep (world,0.05); /* remove all contact joints */ dJointGroupEmpty (contactgroup); } dsSetColor (1,1,0); dsSetTexture (DS_WOOD); for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS); }
void CPHContactBodyEffector::Apply() { const dReal* linear_velocity =dBodyGetLinearVel(m_body); dReal linear_velocity_smag =dDOT(linear_velocity,linear_velocity); dReal linear_velocity_mag =_sqrt(linear_velocity_smag); dReal effect =10000.f*m_recip_flotation*m_recip_flotation; dMass mass; dBodyGetMass(m_body,&mass); dReal l_air=linear_velocity_mag*effect;//force/velocity !!! if(l_air>mass.mass/fixed_step) l_air=mass.mass/fixed_step;//validate if(!fis_zero(l_air)) { dVector3 force={ -linear_velocity[0]*l_air, -linear_velocity[1]*l_air, -linear_velocity[2]*l_air, 0.f }; if(!m_material->Flags.is(SGameMtl::flPassable)) { dVector3& norm=m_contact.geom.normal; accurate_normalize(norm); dMass m; dBodyGetMass(m_body,&m); dReal prg=1.f*dDOT(force,norm);//+dDOT(linear_velocity,norm)*m.mass/fixed_step force[0]-=prg*norm[0]; force[1]-=prg*norm[1]; force[2]-=prg*norm[2]; } dBodyAddForce( m_body, force[0], force[1], force[2] ); } dBodySetData(m_body,NULL); }
void YGEBodyAsset::update(long delta){ if(!hasBody){ createBody(); } else { double second = delta / 1000000.0f; double m = 1; //second * 1000; dBodySetAngularDamping(bodyId, angularDamping * second); dBodySetLinearDamping(bodyId, linearDamping * second); dBodyAddTorque(bodyId, torqueAccum.x * m, torqueAccum.y * m, torqueAccum.z * m); dBodyAddForce(bodyId, forceAccum.x * m, forceAccum.y * m, forceAccum.z * m); const dReal* pos = dBodyGetPosition (bodyId); const dReal* rot = dBodyGetQuaternion(bodyId); parent->setPosition(YGEMath::Vector3(pos[0], pos[1], pos[2])); parent->setOrientation(YGEMath::Quaternion(rot[0], rot[1], rot[2], rot[3])); } }
void base::passive_control(void) { float tmp; up=check_state(); dBodyGetRelPointVel(body,0,0,0,pointrelvel); //get body's speed if(last>1 && !up) tmp=pointrelvel[0]/speed; else if(!up)tmp=0.03; //stand anim speed else tmp=0.3; //jump anim speed if(tmp>=0)calc_speed+=tmp; else calc_speed+=(tmp*-1); if(up) { if(last==0 || last==2) last=4; if(last==1 || last==3) last=5; } if(!up) //idle dBodyAddForce(body,-pointrelvel[0]/1.5,0,0); //for circle only if(pointrelvel[0]<=1 && pointrelvel[0]>=-1 && !up) //stand animation if(last!=0 && last!=1) last-=2; if(calc_speed>=1) { translated_val+=(double)1/texture[last].n; calc_speed=0; } if(last_prev!=last) translated_val=0; last_prev=last; //~ hack_2d(); //~ if(keystates['e']) rot+=5; //~ if(keystates['q']) rot-=5; }
void controller::gravityCompensation() { dBodyAddForce(body_bag->getBackLink1Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink1Length(), 0.0); dBodyAddForce(body_bag->getBackLink2Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink2Length(), 0.0); dBodyAddForce(body_bag->getBackLink3Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink3Length(), 0.0); dBodyAddForce(body_bag->getBackLink4Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink4Length(), 0.0); dBodyAddForce(body_bag->getBackLink5Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink5Length(), 0.0); dBodyAddForce(body_bag->getBackLink6Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getBackLink6Length(), 0.0); /*//Neck and head float force = 9.810*body_bag->getDensity()*(body_bag->getNnhLinkLength(0) + body_bag->getNnhLinkLength(1) + body_bag->getNnhLinkLength(2) + body_bag->getNnhLinkLength(3)); dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0); dBodyAddForce(body_bag->getNnhLink1Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getNnhLinkLength(0), 0.0); dBodyAddForce(body_bag->getNnhLink2Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getNnhLinkLength(1), 0.0); dBodyAddForce(body_bag->getNnhLink3Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getNnhLinkLength(2), 0.0); dBodyAddForce(body_bag->getNnhLink4Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getNnhLinkLength(3), 0.0); //Tail dBodyAddForce(body_bag->getTailLink1Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getTailLinkLength(0), 0.0); dBodyAddForce(body_bag->getTailLink2Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getTailLinkLength(1), 0.0); dBodyAddForce(body_bag->getTailLink3Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getTailLinkLength(2), 0.0); dBodyAddForce(body_bag->getTailLink4Body(), 0.0, 9.810*body_bag->getDensity()*body_bag->getTailLinkLength(3), 0.0);*/ /*//Front left foot dBodyAddForce(body_bag->getFootLinkBody(0,0), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(0,1), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(0,2), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(0,3), 0.0, 98.1, 0.0); //Front right foot dBodyAddForce(body_bag->getFootLinkBody(1,0), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(1,1), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(1,2), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(1,3), 0.0, 98.1, 0.0); //Back left foot dBodyAddForce(body_bag->getFootLinkBody(2,0), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(2,1), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(2,2), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(2,3), 0.0, 98.1, 0.0); //Back right foot dBodyAddForce(body_bag->getFootLinkBody(3,0), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(3,1), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(3,2), 0.0, 98.1, 0.0); dBodyAddForce(body_bag->getFootLinkBody(3,3), 0.0, 98.1, 0.0);*/ }
void RigidBody::addForce(const ngl::Vec3 &_f) { dBodyAddForce(m_id,_f.m_x,_f.m_y,_f.m_z); }