void Machine::reset(void) { dMatrix3 R; if(this==&machine[0]) { dRFromAxisAndAngle(R, 0, 0, 1, -M_PI/2.0); dBodySetPosition(body[0], -court.x/2, 0, 26.5); dBodySetRotation(body[0], R); dBodySetPosition(body[1], -court.x/2+17, 0, 26.5); dBodySetRotation(body[1], R); int i; for(i=0; i<3; i++) dBodySetRotation(wheel[i], R); dBodySetPosition(wheel[0], 12-court.x/2, 0, 26); dBodySetPosition(wheel[1], -7-court.x/2, 6, 26); dBodySetPosition(wheel[2], -7-court.x/2, -6, 26); } else { dRFromAxisAndAngle(R, 0, 0, 1, M_PI/2.0); dBodySetPosition(body[0], court.x/2, 0, 26.5); dBodySetRotation(body[0], R); dBodySetPosition(body[1], court.x/2-17, 0, 26.5); dBodySetRotation(body[1], R); int i; for(i=0; i<3; i++) dBodySetRotation(wheel[i], R); dBodySetPosition(wheel[0], -12+court.x/2, 0, 26); dBodySetPosition(wheel[1], 7+court.x/2, -6, 26); dBodySetPosition(wheel[2], 7+court.x/2, 6, 26); } steer=0; pushtime=0; energy=4; adjuststeer(); dBodySetLinearVel(body[0], 0, 0, 0); dBodySetLinearVel(body[1], 0, 0, 0); dBodySetAngularVel(body[0], 0, 0, 0); dBodySetAngularVel(body[1], 0, 0, 0); int i; for(i=0; i<3; i++) { dBodySetLinearVel(wheel[i], 0, 0, 0); dBodySetAngularVel(wheel[i], 0, 0, 0); } upsidedowntime=0; shieldcount=0; meshatekcount=0; powerupammo=0; powerupcount=0; powerupcharge=0; }
void atualizarVelocidadesAngulares() { double novaVelocidadeAngular; for(int i = 0; i < ROBOTS; i++) { novaVelocidadeAngular = robot[i].pegarVelocidadeAngular() * TQ; dBodySetAngularVel(robot[i].body[0], 0, 0, novaVelocidadeAngular); dBodySetAngularVel(robot[i].body[1], 0, 0, novaVelocidadeAngular); dBodySetAngularVel(robot[i].wheel[0], 0, 0, novaVelocidadeAngular); dBodySetAngularVel(robot[i].wheel[1], 0, 0, novaVelocidadeAngular); } }
/** * @brief Set angular velocity of the parts * * @param x angular velocity around x axis * @param y angular velocity around y axis * @param z angular velocity around z axis */ void SParts::setAngularVelocity(dReal x,dReal y,dReal z) { //TODO: using maxAngularVel // call API of the ODE dBodySetAngularVel(m_odeobj->body(),x,y,z); //const dReal *dat = dBodyGetAngularVel(m_odeobj->body()); }
void GLWidget::moveBallHere() { ssl->ball->setBodyPosition(ssl->cursor_x,ssl->cursor_y,cfg->v_BallRadius->getValue()*2); dBodySetLinearVel(ssl->ball->body, 0.0, 0.0, 0.0); dBodySetAngularVel(ssl->ball->body, 0.0, 0.0, 0.0); }
void ompl::control::OpenDEStateSpace::writeState(const base::State *state) const { const auto *s = state->as<StateType>(); for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i) { unsigned int _i4 = i * 4; double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4; dBodySetPosition(env_->stateBodies_[i], s_pos[0], s_pos[1], s_pos[2]); double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4; dBodySetLinearVel(env_->stateBodies_[i], s_vel[0], s_vel[1], s_vel[2]); double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values; ++_i4; dBodySetAngularVel(env_->stateBodies_[i], s_ang[0], s_ang[1], s_ang[2]); const base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4); dQuaternion q; q[0] = s_rot.w; q[1] = s_rot.x; q[2] = s_rot.y; q[3] = s_rot.z; dBodySetQuaternion(env_->stateBodies_[i], q); } }
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; } }
static void reset_state(void) { float sx=-4, sy=-4, sz=2; dQuaternion q; dQFromAxisAndAngle (q,1,0,0,M_PI*0.5); #ifdef BOX dBodySetPosition (boxbody, sx, sy+1, sz); dBodySetLinearVel (boxbody, 0,0,0); dBodySetAngularVel (boxbody, 0,0,0); dBodySetQuaternion (boxbody, q); #endif #ifdef CYL dBodySetPosition (cylbody, sx, sy, sz); dBodySetLinearVel (cylbody, 0,0,0); dBodySetAngularVel (cylbody, 0,0,0); dBodySetQuaternion (cylbody, q); #endif }
void ODE_Link::setAbsVelocity(hrp::Vector3& v, hrp::Vector3& w){ dBodySetAngularVel(bodyId, w[0], w[1], w[2]); hrp::Vector3 p; hrp::Matrix33 R; getTransform(p, R); hrp::Vector3 cpos(R*C); hrp::Vector3 _v(v + w.cross(cpos)); dBodySetLinearVel(bodyId, _v[0], _v[1], _v[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]); }
void Robot::resetRobot() { resetSpeeds(); dBodySetLinearVel(chassis->body,0,0,0); dBodySetAngularVel(chassis->body,0,0,0); dBodySetLinearVel(dummy->body,0,0,0); dBodySetAngularVel(dummy->body,0,0,0); dBodySetLinearVel(kicker->box->body,0,0,0); dBodySetAngularVel(kicker->box->body,0,0,0); for (int i=0;i<4;i++) { dBodySetLinearVel(wheels[i]->cyl->body,0,0,0); dBodySetAngularVel(wheels[i]->cyl->body,0,0,0); } float x,y; getXY(x,y); setXY(x,y); if (m_dir==-1) setDir(180); else setDir(0); }
static void reset_ball(void) { float sx=0.0f, sy=3.40f, sz=7.15; dQuaternion q; dQSetIdentity(q); dBodySetPosition (sphbody, sx, sy, sz); dBodySetQuaternion(sphbody, q); dBodySetLinearVel (sphbody, 0,0,0); dBodySetAngularVel (sphbody, 0,0,0); }
void FixBody(dBodyID body,float ext_param,float mass_param) { dMass m; dMassSetSphere(&m,1.f,ext_param); dMassAdjust(&m,mass_param); dBodySetMass(body,&m); dBodySetGravityMode(body,0); dBodySetLinearVel(body,0,0,0); dBodySetAngularVel(body,0,0,0); dBodySetForce(body,0,0,0); dBodySetTorque(body,0,0,0); }
void CPHActivationShape::CutVelocity(float l_limit,float /*a_limit*/) { dVector3 limitedl,diffl; if(dVectorLimit(dBodyGetLinearVel(m_body),l_limit,limitedl)) { dVectorSub(diffl,limitedl,dBodyGetLinearVel(m_body)); dBodySetLinearVel(m_body,diffl[0],diffl[1],diffl[2]); dBodySetAngularVel(m_body,0.f,0.f,0.f); dxStepBody(m_body,fixed_step); dBodySetLinearVel(m_body,limitedl[0],limitedl[1],limitedl[2]); } }
/*** キーボードコマンドの処理関数 ***/ static void command(int cmd) { switch (cmd) { case '1': float xyz1[3],hpr1[3]; dsGetViewpoint(xyz1,hpr1); printf("xyz=%4.2f %4.2f %4.2f ",xyz1[0],xyz1[1],xyz1[2]); printf("hpr=%6.2f %6.2f %5.2f \n",hpr1[0],hpr1[1],hpr1[2]); break; case 'k': wheel[3].v -= 0.8; break; // 後進 case 's': wheel[0].v = wheel[1].v = wheel[2].v = wheel[3].v = 0.0; // 停止 break; case 'x': // ゴロシュート { const dReal *bp = dBodyGetPosition(ball.body); const dReal *p = dBodyGetPosition(wheel[0].body); const dReal *R = dBodyGetRotation(base.body); dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1], 2)); if (dist < 0.3) { dReal vmax = POWER * 0.01 * 8.0; dBodySetLinearVel(ball.body,vmax * R[0],vmax * R[4], 0.0); // printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]); } } break; case 'l': // ループシュート { const dReal *bp = dBodyGetPosition(ball.body); const dReal *p = dBodyGetPosition(wheel[0].body); const dReal *R = dBodyGetRotation(base.body); dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1],2)); if (dist < 1.0) { dReal vmax45 = POWER * 0.01 * 8.0 / sqrt(2.0); dBodySetLinearVel(ball.body,vmax45 * R[0],vmax45 * R[4], vmax45); // printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]); } } break; case 'b': dBodySetPosition(ball.body,0,0,0.14+offset_z); dBodySetLinearVel(ball.body,0,0,0); dBodySetAngularVel(ball.body,0,0,0); break; } }
void dRigidBodyArraySetAngularVel(dRigidBodyArrayID bodyArray, dReal ax, dReal ay, dReal az) { dBodyID center = bodyArray->center; const dReal *p0 = dBodyGetPosition(center); dVector3 omega = {ax, ay, az}; for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) { dBodyID body = dRigidBodyArrayGet(bodyArray, i); const dReal *p = dBodyGetPosition(body); dVector3 pdot, r; dOP(r, -, p, p0); dCalcVectorCross3(pdot, omega, r); dBodySetLinearVel(body, pdot[0], pdot[1], pdot[2]); dBodySetAngularVel(body, ax, ay, az); } }
static void reset_ball(void) { float sx=0.0f, sy=3.40f, sz=6.80f; #if defined(_MSC_VER) && defined(dDOUBLE) sy -= 0.01; // Cheat, to make it score under win32/double #endif dQuaternion q; dQSetIdentity(q); dBodySetPosition (sphbody, sx, sy, sz); dBodySetQuaternion(sphbody, q); dBodySetLinearVel (sphbody, 0,0,0); dBodySetAngularVel (sphbody, 0,0,0); }
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; }
void resetBall(dBodyID b, unsigned idx) { dBodySetPosition(b, 0.5*track_len*cos(track_incl) // Z - 0.5*track_height*sin(track_incl) - ball_radius, // X balls_sep*idx, // Y track_elevation + ball_radius// Z + 0.5*track_len*sin(track_incl) + 0.5*track_height*cos(track_incl)); dMatrix3 r = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0}; dBodySetRotation(b, r); dBodySetLinearVel(b, 0, 0, 0); dBodySetAngularVel(b, 0, 0, 0); }
bool Primitive::restore(FILE* f){ Pose pose; Pos vel; Pos avel; if ( fread ( pose.ptr() , sizeof ( Pose::value_type), 16, f ) == 16 ) if( fread ( vel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 ) if( fread ( avel.ptr() , sizeof ( Pos::value_type), 3, f ) == 3 ){ setPose(pose); if(body){ dBodySetLinearVel(body,vel.x(),vel.y(),vel.z()); dBodySetAngularVel(body,avel.x(),avel.y(),avel.z()); } return true; } fprintf ( stderr, "Primitve::restore: cannot read primitive from data\n" ); return false; }
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); } } }
void Simulator::SetCurrentState(const 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 int offset = i * 13; const dReal q[] = {s[offset + 3], s[offset + 4], s[offset + 5], s[offset + 6]}; dBodySetPosition (m_robotBodies[i], s[offset], s[offset + 1], s[offset + 2]); dBodySetQuaternion(m_robotBodies[i], q); dBodySetLinearVel (m_robotBodies[i], s[offset + 7], s[offset + 8], s[offset + 9]); dBodySetAngularVel(m_robotBodies[i], s[offset + 10], s[offset + 11], s[offset + 12]); } dRandSetSeed((long unsigned int) (s[n * 13])); }
void CDynamics3DEntity::Reset() { /* Clear force and torque on the body */ dBodySetForce(m_tBody, 0.0f, 0.0f, 0.0f); dBodySetTorque(m_tBody, 0.0f, 0.0f, 0.0f); /* Clear speeds */ dBodySetLinearVel(m_tBody, 0.0f, 0.0f, 0.0f); dBodySetAngularVel(m_tBody, 0.0f, 0.0f, 0.0f); /* Reset position */ const CVector3& cPosition = GetEmbodiedEntity().GetInitPosition(); dBodySetPosition(m_tBody, cPosition.GetX(), cPosition.GetY(), cPosition.GetZ()); /* Reset orientation */ const CQuaternion& cQuaternion = GetEmbodiedEntity().GetInitOrientation(); dQuaternion tQuat = { cQuaternion.GetW(), cQuaternion.GetX(), cQuaternion.GetY(), cQuaternion.GetZ() }; dBodySetQuaternion(m_tBody, tQuat); }
void AvatarConstruction::OnAccelerate ( Real Force, Real SideForce, Real UpForce ) { if(!OnGround) return; if(UpForce > 0 ) { if( (timeGetTime()-LastJumpTime) < 1000 ) UpForce = 0; else LastJumpTime = timeGetTime(); } const dReal* currentSpeed = dBodyGetLinearVel( ObjectList[0].Body ); Vector3D v, v2; v.x = currentSpeed[0]; v.y = 0; v.z = currentSpeed[2]; if( UpForce != 0 ) dBodySetLinearVel( ObjectList[0].Body, v.x, UpForce, v.z ); if(Force >0 || SideForce>0 || UpForce > 0) ForceEnable= true; float m=(float)sqrt(Force*Force+SideForce*SideForce); Vector3D r ( SideForce, 0, Force ); r.Rotatey( RADIAN(-90 )); dBodySetAngularVel( ObjectList[0].Body, r.x, 0, r.z ); if( SideForce != 0 || Force != 0) Moving = true; }
void Player::Update(scalar dt) { const dReal * pos = dBodyGetPosition(bodyId); position.x = pos[0]; position.y = pos[1]; position.z = pos[2]; if (moveForward) { TGen::Vector3 hej = direction * 2.6f * dt; dBodyEnable(bodyId); dBodySetForce(bodyId, hej.x, hej.y, hej.z); //dBodySetLinearVel(bodyId, hej.x, hej.y, hej.z); } else { // dBodySetLinearVel(bodyId, 0.0f, 0.0f, 0.0f); } if (spinLeft) lookDir += dt * 2.0f; if (spinRight) lookDir -= dt * 2.0f; direction.x = sin(lookDir); direction.z = cos(lookDir); dMatrix3 ident; dRSetIdentity(ident); dBodySetRotation(bodyId, ident); dBodySetAngularVel(bodyId, 0.0f, 0.0f, 0.0f); dBodySetTorque(bodyId, 0.0f, 0.0f, 0.0f); //std::cout << "player: " << std::string(position) << std::endl; }
void Robot::Kicker::step() { if (kicking) { box->setColor(1,0.3,0); kickstate--; if (kickstate<=0) kicking = false; } else if (rolling!=0) { box->setColor(1,0.7,0); if (isTouchingBall()) { float fx,fy,fz; rob->chassis->getBodyDirection(fx,fy,fz); fz = 0.6*sqrt(fx*fx + fy*fy); // Increasing the power of dribbler. fx/=fz;fy/=fz; if (rolling==-1) {fx=-fx;fy=-fy;} rob->getBall()->tag = rob->getID(); float vx,vy,vz; float bx,by,bz; float kx,ky,kz; rob->chassis->getBodyDirection(vx,vy,vz); rob->getBall()->getBodyPosition(bx,by,bz); box->getBodyPosition(kx,ky,kz); float yy = -10*((-(kx-bx)*vy + (ky-by)*vx)) / rob->cfg->robotSettings.KickerWidth; float dir = 1; if (yy>0) dir = -1.0f; dBodySetAngularVel(rob->getBall()->body,fy*rob->cfg->robotSettings.RollerTorqueFactor*1400,-fx*rob->cfg->robotSettings.RollerTorqueFactor*1400,0); //dBodyAddTorque(rob->getBall()->body,fy*rob->cfg->ROLLERTORQUEFACTOR(),-fx*rob->cfg->ROLLERTORQUEFACTOR(),0); dBodyAddTorque(rob->getBall()->body,yy*fx*rob->cfg->robotSettings.RollerPerpendicularTorqueFactor,yy*fy*rob->cfg->robotSettings.RollerPerpendicularTorqueFactor,0); } } else box->setColor(0.9,0.9,0.9); }
void RigidBody::setAngularVelocity(const ngl::Vec3 &_v) { dBodySetAngularVel(m_id,_v.m_x,_v.m_y,_v.m_z); }
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; } }
void setPositionBodies (int val) { const dVector3 POS1 = {0,0,1.5,0}; const dVector3 POS2 = {0,0,1.5,0}; const dVector3 ANCHOR = {0,0,1.5,0}; for (int i=0; i<3; ++i) { pos1[i] = POS1[i]; pos2[i] = POS2[i]; anchor[i] = ANCHOR[i]; } if (body[BODY1]) { dBodySetLinearVel (body[BODY1], 0,0,0); dBodySetAngularVel (body[BODY1], 0,0,0); } if (body[BODY2]) { dBodySetLinearVel (body[BODY2], 0,0,0); dBodySetAngularVel (body[BODY2], 0,0,0); } switch (val) { case 3: pos1[Z] += -0.5; anchor[Z] -= 0.25; break; case 2: pos1[Z] -= 0.5; anchor[Z] -= 0.5; break; case 1: pos1[Z] += -0.5; break; default: // This is also case 0 // Nothing to be done break; } const dMatrix3 R = { 1,0,0,0, 0,1,0,0, 0,0,1,0 }; if (body[BODY1]) { dBodySetPosition (body[BODY1], pos1[X], pos1[Y], pos1[Z]); dBodySetRotation (body[BODY1], R); } if (body[BODY2]) { dBodySetPosition (body[BODY2], pos2[X], pos2[Y], pos2[Z]); dBodySetRotation (body[BODY2], R); } if (joint) { joint->attach (body[BODY1], body[BODY2]); if (joint->getType() == dJointTypePiston) dJointSetPistonAnchor(joint->id(), anchor[X], anchor[Y], anchor[Z]); } }
void ODE_Particle::setAngularVel(dReal wx, dReal wy, dReal wz) { dBodySetAngularVel(body, wx,wy,wz); }
static void command (int cmd) { switch (cmd) { case 'a': case 'A': speed += 0.3; break; case 'z': case 'Z': speed -= 0.3; break; case ',': turn += 0.1; if (turn > 0.3) turn = 0.3; break; case '.': turn -= 0.1; if (turn < -0.3) turn = -0.3; break; case ' ': speed = 0; turn = 0; break; case 'f': case 'F': doFast = !doFast; break; case '+': dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1); break; case '-': dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1); break; case 'r': case 'R': resetSimulation(); break; case '[': cannon_angle += 0.1; break; case ']': cannon_angle -= 0.1; break; case '1': cannon_elevation += 0.1; break; case '2': cannon_elevation -= 0.1; break; case 'x': case 'X': { 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}; for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2]; dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]); dReal force = 10; dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]); dBodySetAngularVel (cannon_ball_body,0,0,0); break; } } }