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