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