bool VelocityLimit()
{
const float *linear_velocity =dBodyGetLinearVel(m_body);
//limit velocity
bool ret=false;
if(dV_valid(linear_velocity))
{
dReal mag;
Fvector vlinear_velocity;vlinear_velocity.set(cast_fv(linear_velocity));
mag=_sqrt(linear_velocity[0]*linear_velocity[0]+linear_velocity[2]*linear_velocity[2]);//
if(mag>l_limit)
{
dReal f=mag/l_limit;
//dBodySetLinearVel(m_body,linear_velocity[0]/f,linear_velocity[1],linear_velocity[2]/f);///f
vlinear_velocity.x/=f;vlinear_velocity.z/=f;
ret=true;
}
mag=_abs(linear_velocity[1]);
if(mag>y_limit)
{
vlinear_velocity.y=linear_velocity[1]/mag*y_limit;
ret=true;
}
dBodySetLinearVel(m_body,vlinear_velocity.x,vlinear_velocity.y,vlinear_velocity.z);
return ret;
}
else
{
dBodySetLinearVel(m_body,m_safe_velocity[0],m_safe_velocity[1],m_safe_velocity[2]);
return true;
}
}
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 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 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 createInvisibleHead( float* pos )
{
dMatrix3 head_orientation;
dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);
//position and orientation
head.Body = dBodyCreate(World);
dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
dBodySetRotation(head.Body, head_orientation);
dBodySetLinearVel(head.Body, 0, 0, 0);
dBodySetData(head.Body, (void *)0);
//mass
dMass head_mass;
dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
dBodySetMass(head.Body, &head_mass);
//geometry
head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
dGeomSetBody(head.Geom, head.Body);
//fixed joint
invis_box_joint = dJointCreateFixed(World, jointgroup);
dJointAttach(invis_box_joint, body.Body, head.Body);
dJointSetFixed(invis_box_joint);
}
/*
=================================================================================
createFixedLeg
Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius,
dReal length)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//fixed joint
joint = dJointCreateFixed(World, jointgroup);
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetFixed(joint);
}
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 atualizarVelocidadeBola()
{
if (bola.podeControlar())
{
dBodySetLinearVel(bola.corpo, bola.pegarVelocidadeX(), bola.pegarVelocidadeY(), 0);
}
}
void dRigidBodyArrayAddLinearVel(dRigidBodyArrayID bodyArray, dReal lx, dReal ly, dReal lz) {
for(size_t i = 0; i < dRigidBodyArraySize(bodyArray); i++) {
dBodyID body = dRigidBodyArrayGet(bodyArray, i);
const dReal *v = dBodyGetLinearVel(body);
dBodySetLinearVel(body, v[0] + lx, v[1] + ly, v[2] + lz);
}
}
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 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]);
}
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 atualizarVelocidadesLineares()
{
double vx, vy;
const dReal *rot;
for(int i = 0; i < ROBOTS; i++)
{
rot = dBodyGetRotation(robot[i].body[0]); // rot[1] retorna o valor como o negativo do valor real
vx = robot[i].pegarVelocidadeLinear() * rot[0] * TQ/3.0;
vy = -robot[i].pegarVelocidadeLinear() * rot[1] * TQ/3.0; //Como rot[1] tem sinal trocado eh necessario o sinal - para corrigir isso.
dBodySetLinearVel(robot[i].body[0], vx, vy, 0);
dBodySetLinearVel(robot[i].body[1], vx, vy, 0);
dBodySetLinearVel(robot[i].wheel[0], vx, vy, 0);
dBodySetLinearVel(robot[i].wheel[1], vx, vy, 0);
}
}
IoObject *IoODEBody_setLinearVelocity(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);
dBodySetLinearVel(BODYID, x, y, z);
return self;
}
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 Sphere::MakeBody(dWorldID world)
{
iBody = dBodyCreate(world);
dMassSetSphereTotal(&iMass,iM,iRadius);
dBodySetMass(iBody,&iMass);
dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z);
dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z);
disabledSteps = 0;
dBodySetData(iBody,data);
}
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);
}
}
bool Primitive::limitLinearVel(double maxVel){
// check for maximum speed:
if(!body) return false;
const double* vel = dBodyGetLinearVel( 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;
dBodySetLinearVel(body, vel[0]/scaling, vel[1]/scaling, vel[2]/scaling);
return true;
}else
return false;
}
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);
}
/*
=================================================================================
createUniversalLeg
Use parameters to create leg body/geom and attach to body with universal joint
**Warning**
mass is not set
=================================================================================
*/
void createUniversalLeg(ODEObject &leg,
ODEObject &bodyAttachedTo,
dJointID& joint,
dReal xPos, dReal yPos, dReal zPos,
dReal xRot, dReal yRot, dReal zRot,
dReal radius, dReal length,
dReal maxAngle, dReal minAngle,
dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
dMatrix3 legOrient;
dRFromEulerAngles(legOrient, xRot, yRot, zRot);
//position and orientation
leg.Body = dBodyCreate(World);
dBodySetPosition(leg.Body, xPos, yPos, zPos);
dBodySetRotation(leg.Body, legOrient);
dBodySetLinearVel(leg.Body, 0, 0, 0);
dBodySetData(leg.Body, (void *)0);
//mass
dMass legMass;
dMassSetCapsule(&legMass, 1, 3, radius, length);
//dBodySetMass(leg.Body, &legMass);
//geometry
leg.Geom = dCreateCapsule(Space, radius, length);
dGeomSetBody(leg.Geom, leg.Body);
//universal joint
joint = dJointCreateUniversal(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetUniversalAxis1(joint, 0, 0, 1);
dJointSetUniversalAxis2(joint, 0, 1, 0);
//Max and min angles
dJointSetUniversalParam(joint, dParamHiStop, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop, minAngle);
dJointSetUniversalParam(joint, dParamHiStop2, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop2, minAngle);
}
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 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);
}
void Object::MakeBody(dWorldID world)
{
iBody = dBodyCreate(world);
dBodySetQuaternion(iBody,q);
dBodySetPosition(iBody,iPosition.x,iPosition.y,iPosition.z);
dBodySetLinearVel(iBody,iVel.x,iVel.y,iVel.z);
dMassSetBox(&iMass,iM,iSize.x,iSize.y,iSize.z);
dMassAdjust(&iMass,iM/2.0);
dBodySetMass(iBody,&iMass);
disabledSteps = 0;
dBodySetAutoDisableFlag(iBody,1);
dBodySetData(iBody,data);
dBodyDisable(iBody);
};
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 Balaenidae::launch(Manta* m)
{
m->inert = true;
m->setStatus(2);
m->elevator = +12;
struct controlregister c;
c.thrust = 1500.0f/(-10.0);
c.pitch = 12;
m->setControlRegisters(c);
m->setThrottle(1500.0f);
Vec3f p = m->getPos();
p[1] += 10;
m->setPos(p);
dBodySetPosition(m->getBodyID(),p[0],p[1],p[2]);
// @FIXME: Fix the rotation of Manta after it is launched (due to the existence of angularPos in Manta).
Vec3f f;
f = m->getForward();
f = f*500;
dBodySetLinearVel(m->getBodyID(),f[0],f[1],f[2]);
}
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);
}
