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]);
}
void odeSpringApplyForce( dBodyID b0, dBodyID b1, FVec3 off0, FVec3 off1, FVec3 forceVec, float damping ) {
dReal zero[3] = { 0.0, 0.0, 0.0 };
const dReal *vel0 = (const dReal *)&zero;
const dReal *vel1 = (const dReal *)&zero;
if( b0 ) {
vel0 = dBodyGetLinearVel( b0 );
}
if( b1 ) {
vel1 = dBodyGetLinearVel( b1 );
}
FVec3 relVel( (float)vel0[0]-(float)vel1[0], (float)vel0[1]-(float)vel1[1], (float)vel0[2]-(float)vel1[2] );
float a = relVel.dot( forceVec );
float b = forceVec.dot( forceVec );
FVec3 dampingVec = forceVec;
if( fabsf(b) > 1e-5f ) {
dampingVec.mul( -damping * a / b );
}
forceVec.add( dampingVec );
if( b0 ) {
dBodyAddForceAtRelPos( b0, forceVec.x, forceVec.y, forceVec.z, off0.x, off0.y, off0.z );
}
if( b1 ) {
dBodyAddForceAtRelPos( b1, -forceVec.x, -forceVec.y, -forceVec.z, off1.x, off1.y, off1.z );
}
}
//body - body case
float E_NLD(dBodyID b1,dBodyID b2,const dReal* norm)// norm - from 2 to 1
{
dMass m1,m2;
dBodyGetMass(b1,&m1);dBodyGetMass(b2,&m2);
const dReal* vel1 =dBodyGetLinearVel(b1);
const dReal* vel2 =dBodyGetLinearVel(b2);
dReal vel_pr1=dDOT(vel1,norm);
dReal vel_pr2=dDOT(vel2,norm);
if(vel_pr1>vel_pr2) return 0.f; //exit if the bodies are departing
dVector3 impuls1={vel1[0]*m1.mass,vel1[1]*m1.mass,vel1[2]*m1.mass};
dVector3 impuls2={vel2[0]*m2.mass,vel2[1]*m2.mass,vel2[2]*m2.mass};
dVector3 c_mas_impuls={impuls1[0]+impuls2[0],impuls1[1]+impuls2[1],impuls1[2]+impuls2[2]};
dReal cmass=m1.mass+m2.mass;
dVector3 c_mass_vel={c_mas_impuls[0]/cmass,c_mas_impuls[1]/cmass,c_mas_impuls[2]/cmass};
dReal c_mass_vel_prg=dDOT(c_mass_vel,norm);
dReal kin_energy_start=vel_pr1*vel_pr1*m1.mass/2.f+vel_pr2*vel_pr2*m2.mass/2.f;
dReal kin_energy_end=c_mass_vel_prg*c_mass_vel_prg*cmass/2.f;
return (kin_energy_start-kin_energy_end);
}
void checktest6(unsigned long timer)
{
static bool isOffshoring = false;
if (timer==100)
{
Balaenidae *b = (Balaenidae*)entities[0];
struct controlregister c;
memset(&c,0,sizeof(struct controlregister));
c.thrust = 10000.0f;
c.pitch = 0;
c.roll = 10;
b->stop();
b->setControlRegisters(c);
b->setThrottle(1000.0f);
}
if (timer >100 )
{
Balaenidae *b = (Balaenidae*)entities[0];
if (b->getStatus()==Balaenidae::OFFSHORING)
{
isOffshoring = true;
}
}
if (timer==3800)
{
Balaenidae *b = (Balaenidae*)entities[0];
Vehicle *_b = entities[0];
Vec3f val = _b->getPos();
dReal *v = (dReal *)dBodyGetLinearVel(_b->getBodyID());
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
dReal *av = (dReal *)dBodyGetLinearVel(_b->getBodyID());
Vec3f vav;
vav[0]= av[0];vav[1] = av[1]; vav[2] = av[2];
if (!isOffshoring){
printf("Test failed: Carrier never offshored.\n");
endWorldModelling();
exit(-1);
}
if (b->getStatus() != Balaenidae::SAILING || vav.magnitude()>100)
{
printf("Test failed: Carrier is still moving.\n");
endWorldModelling();
exit(-1);
} else {
printf("Test passed OK!\n");
endWorldModelling();
exit(1);
}
}
}
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]);
}
}
int dcTriListCollider::CollideBox(dxGeom* Box, int Flags, dContactGeom* Contacts, int Stride)
{
Fvector AABB;
dVector3 BoxSides;
dGeomBoxGetLengths(Box,BoxSides);
dReal* R=const_cast<dReal*>(dGeomGetRotation(Box));
AABB.x=(dFabs(BoxSides[0]*R[0])+dFabs(BoxSides[1]*R[1])+dFabs(BoxSides[2]*R[2]))/2.f +10.f*EPS_L;
AABB.y=(dFabs(BoxSides[0]*R[4])+dFabs(BoxSides[1]*R[5])+dFabs(BoxSides[2]*R[6]))/2.f +10.f*EPS_L;
AABB.z=(dFabs(BoxSides[0]*R[8])+dFabs(BoxSides[1]*R[9])+dFabs(BoxSides[2]*R[10]))/2.f+10.f*EPS_L;
dBodyID box_body=dGeomGetBody(Box);
if(box_body) {
const dReal*velocity=dBodyGetLinearVel(box_body);
AABB.x+=dFabs(velocity[0])*0.04f;
AABB.y+=dFabs(velocity[1])*0.04f;
AABB.z+=dFabs(velocity[2])*0.04f;
}
BoxTri bt(*this);
return dSortTriPrimitiveCollide
(bt,
Box,
Geometry,
Flags,
Contacts,
Stride,
AABB
);
}
// Changed from getVelocity: by inamura on 2013-12-30
void SParts::getLinearVelocity(Vector3d &v)
{
const dReal *avel = dBodyGetLinearVel(m_odeobj->body());
v.x(avel[0]);
v.y(avel[1]);
v.z(avel[2]);
}
Kinematic PMoveable::odeToKinematic()
{
Kinematic k;
dQuaternion q_result, q_result1, q_base;
float norm;
const dReal *b_info;
const dReal *q_current = dBodyGetQuaternion(body);
q_base[0] = 0; q_base[1] = 0; q_base[2] = 0; q_base[3] = 1;
dQMultiply0(q_result1, q_current, q_base);
dQMultiply2(q_result, q_result1, q_current);
k.orientation_v = Vec3f(q_result[1], q_result[2], q_result[3]);
norm = sqrt(q_result[1] * q_result[1] + q_result[3] * q_result[3]);
if (norm == 0)
k.orientation = 0;
else
k.orientation = atan2(q_result[1] / norm, q_result[3] / norm);
b_info = dBodyGetPosition(body);
k.pos = Vec3f(b_info[0], b_info[1], b_info[2]);
b_info = dBodyGetLinearVel(body);
k.vel = Vec3f(b_info[0], b_info[1], b_info[2]);
return k;
}
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::step()
{
static double lastBallSpeed=-1;
const dReal* ballV = dBodyGetLinearVel(ssl->ball->body);
double ballSpeed = ballV[0]*ballV[0] + ballV[1]*ballV[1] + ballV[2]*ballV[2];
ballSpeed = sqrt(ballSpeed);
lastBallSpeed = ballSpeed;
rendertimer.restart();
m_fps = frames /(time.elapsed()/1000.0);
if (!(frames % ((int)(ceil(cfg->v_DesiredFPS->getValue()))))) {
time.restart();
frames = 0;
}
if (first_time) {ssl->step();first_time = false;}
else {
if (cfg->v_SyncWithGL->getValue())
{
dReal ddt=rendertimer.elapsed()/1000.0;
if (ddt>0.05) ddt=0.05;
ssl->step(ddt);
}
else {
ssl->step(cfg->v_DeltaTime->getValue());
}
}
frames ++;
}
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 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);
}
};
/* 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 cPhysicsObject::Update(durationms_t currentTime)
{
if (bDynamic) {
const dReal* p0 = nullptr;
const dReal* r0 = nullptr;
dQuaternion q;
if (bBody) {
p0 = dBodyGetPosition(body);
r0 = dBodyGetQuaternion(body);
const dReal* v0 = dBodyGetLinearVel(body);
//const dReal *a0=dBodyGetAngularVel(body);
v[0] = v0[0];
v[1] = v0[1];
v[2] = v0[2];
} else {
p0 = dGeomGetPosition(geom);
dGeomGetQuaternion(geom, q);
r0 = q;
// These are static for the moment
v[0] = 0.0f;
v[1] = 0.0f;
v[2] = 0.0f;
}
ASSERT(p0 != nullptr);
ASSERT(r0 != nullptr);
position.Set(p0[0], p0[1], p0[2]);
rotation.SetFromODEQuaternion(r0);
}
}
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;
}
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 Construction::GetVelocity(Vector3D& v)
{
const dReal* velocity = dBodyGetLinearVel ( ObjectList[0].Body );
v.x = velocity[0];
v.y = velocity[1];
v.z = velocity[2];
}
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 Balaenidae::doDynamics(dBodyID body)
{
Vec3f Ft;
Ft[0]=0;Ft[1]=0;Ft[2]=getThrottle();
dBodyAddRelForce(body,Ft[0],Ft[1],Ft[2]);
dBodyAddRelTorque(body,0.0f,Balaenidae::rudder*1000,0.0f);
if (offshoring == 1) {
offshoring=0;
setStatus(Balaenidae::SAILING);
}
else if (offshoring > 0)
{
// Add a retractive force to keep it out of the island.
Vec3f ap = Balaenidae::ap;
setThrottle(0.0);
Vec3f V = ap*(-10000);
dBodyAddRelForce(body,V[0],V[1],V[2]);
offshoring--;
}
// Buyoncy
//if (pos[1]<0.0f)
// dBodyAddRelForce(me,0.0,9.81*20050.0f,0.0);
dReal *v = (dReal *)dBodyGetLinearVel(body);
dVector3 O;
dBodyGetRelPointPos( body, 0,0,0, O);
dVector3 F;
dBodyGetRelPointPos( body, 0,0,1, F);
F[0] = (F[0]-O[0]);
F[1] = (F[1]-O[1]);
F[2] = (F[2]-O[2]);
Vec3f vec3fF;
vec3fF[0] = F[0];vec3fF[1] = F[1]; vec3fF[2] = F[2];
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
speed = vec3fV.magnitude();
VERIFY(speed, me);
vec3fV = vec3fV * 0.02f;
dBodyAddRelForce(body,vec3fV[0],vec3fV[1],vec3fV[2]);
wrapDynamics(body);
}
ngl::Vec3 RigidBody::getLinearVelocity()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=dBodyGetLinearVel(m_id);
return ngl::Vec3(*pos,*(pos+1),*(pos+2));
}
////Energy of non Elastic collision;
//body - static case
float E_NlS(dBodyID body,const dReal* norm,float norm_sign)//if body c.geom.g1 norm_sign + else -
{ //norm*norm_sign - to body
const dReal* vel=dBodyGetLinearVel(body);
dReal prg=-dDOT(vel,norm)*norm_sign;
prg=prg<0.f ? prg=0.f : prg;
dMass mass;
dBodyGetMass(body,&mass);
return mass.mass*prg*prg/2;
}
CVelocityLimiter(dBodyID b,float l,float yl)
{
R_ASSERT(b);
m_body=b;
dVectorSet(m_safe_velocity,dBodyGetLinearVel(m_body));
dVectorSet(m_safe_position,dBodyGetPosition(m_body));
l_limit=l;
y_limit=yl;
}
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 GameObject::GetGameState(GameState &gs)
{
const dReal *velocity = dBodyGetLinearVel(m_body);
gs.m_speed = sqrt(pow(velocity[0], 2) + pow(velocity[2], 2));
// create vectors of differences between positions
gs.m_nearbyMoving = std::vector<WorldPos>();
gs.m_nearbyStatic = std::vector<WorldPos>();
Ogre::Vector3 n = GetLocation();
for (auto& i : m_gw.m_objects) {
Ogre::Vector3 m = i.GetLocation();
double diffX = m.x - n.x;
double diffY = m.z - n.z;
double theta = Ogre::Math::ATan2(diffY, diffX).valueDegrees();
if (i.m_isKinematic) {
gs.m_nearbyStatic.push_back(WorldPos {diffX, diffY, theta});
} else {
gs.m_nearbyMoving.push_back(WorldPos {diffX, diffY, theta});
}
}
// this is a gross O(n) lookup of distance from a path
WorldPos p = {n.x, n.z, 0};
gs.m_distanceFromCenter = m_gw.m_map.getDistanceFromPath(p);
// calculate the distance and angle to the nearest destination
// this also removes destinations that you're very close to
gs.m_distanceFromDestination = 0;
gs.m_deviationAngle = 0;
bool beginPopping = false;
while ((m_pathToDest.size() != 0) && (gs.m_distanceFromDestination < 2.f)) {
if (beginPopping) {
m_pathToDest.pop_back();
} else {
beginPopping = true;
}
WorldPos p = m_pathToDest.back();
double diffX = p.x - n.x;
double diffY = p.y - n.z;
gs.m_distanceFromDestination = Ogre::Math::Sqrt(Ogre::Math::Sqr(diffX) + Ogre::Math::Sqr(diffY));
gs.m_deviationAngle = Ogre::Math::ATan2(diffY, diffX).valueDegrees();
}
// things that we've hit
gs.m_damageSelfInstant = m_collisionAccum;
gs.m_damageSelfTotal = m_totalDamage;
gs.m_damageOthersInstant = 0; // yeah, not using this
gs.m_damageOthersTotal = 0; // ... or this
}
YGEMath::Vector3 YGEBodyAsset::getRelativeVelocity(){
if(hasBody) {
YGEMath::Quaternion q = parent->getOrientation();
const dReal* v = dBodyGetLinearVel(bodyId);
YGEMath::Vector3 z(v[0],v[1],v[2]);
return q.getConjugate().rotateVector(z);
} else {
return YGEMath::Vector3();
}
}
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;
}
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);
}
}
}
int dcTriListCollider::CollideSphere(dxGeom* Sphere, int Flags, dContactGeom* Contacts, int Stride){
const float SphereRadius = dGeomSphereGetRadius(Sphere);
Fvector AABB;
// Make AABB
AABB.x=SphereRadius;
AABB.y=SphereRadius;
AABB.z=SphereRadius;
const dReal*velocity=dBodyGetLinearVel(dGeomGetBody(Sphere));
AABB.x+=dFabs(velocity[0])*0.04f;
AABB.y+=dFabs(velocity[1])*0.04f;
AABB.z+=dFabs(velocity[2])*0.04f;
SphereTri st(*this);
return dSortTriPrimitiveCollide(st,Sphere,Geometry,Flags,Contacts,Stride,
AABB);
}
/*** シミュレーションループ ***/
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();
}