//===========================================================================
void cODEGenericBody::addGlobalForceAtGlobalPos(cVector3d& a_force, cVector3d& a_pos)
{
if (m_ode_body != NULL)
{
dBodyAddForceAtPos(m_ode_body,
a_force.x, a_force.y, a_force.z,
a_pos.x, a_pos.y, a_pos.z);
}
}
int ODEObject::push_handler(const char *path, const char *types,
lo_arg **argv, int argc,
void *data, void *user_data)
{
OscObject *me = static_cast<OscObject*>(user_data);
ODEObject *ode_object = static_cast<ODEObject*>(me->special());
cVector3d(argv[0]->f, argv[1]->f, argv[2]->f).copyto(me->m_force);
dBodyAddForceAtPos(ode_object->body(),
argv[0]->f, argv[1]->f, argv[2]->f,
argv[3]->f, argv[4]->f, argv[5]->f);
return 0;
}
void SpringHook::Step(Real dt)
{
Matrix3 R;
Vector3 t;
Vector3 wp,f;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
wp = R*localpt+t;
f = k*(target-wp);
//cout<<"Target "<<target<<", world point "<<wp<<", force "<<f<<endl;
dBodyAddForceAtPos(body,f.x,f.y,f.z,wp.x,wp.y,wp.z);
}
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 draw(void)
{
// set the background color of the world
cColorf color = camera->getParentWorld()->getBackgroundColor();
glClearColor(color.getR(), color.getG(), color.getB(), color.getA());
// clear the color and depth buffers
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// render world in the window display
camera->renderView(width, height);
// check for any OpenGL errors
GLenum err;
err = glGetError();
if (err != GL_NO_ERROR) printf("Error: %s\n", gluErrorString(err));
// Swap buffers
glutSwapBuffers();
if (inContact >= 0)
{
float x = lastContactPoint.x;
float y = lastContactPoint.y;
float z = lastContactPoint.z;
float fx = -force_scale*lastForce.x ;
float fy = -force_scale*lastForce.y ;
float fz = -force_scale*lastForce.z ;
if (loading == 0)
dBodyAddForceAtPos(obj[inContact].body,fx,fy,fz,x,y,z);
}
simStep();
}
void RigidBody::addForceAtPosition(const ngl::Vec3 &_f,const ngl::Vec3 &_p)
{
dBodyAddForceAtPos(m_id,_f.m_x,_f.m_y,_f.m_z,_p.m_x,_p.m_y,_p.m_z);
}
void CProtoHapticDoc::SimulationStep()
{
int elapsed= clock() - m_lastSimStep;
int steps= (int)((((float)elapsed)*m_simSpeed)*0.1f);
if(steps<1) {
m_lastSimStep= clock();
return;
}
// collision detection
for(int i= 0; i<m_shapeCount; i++) {
if(m_shapes[i]->isCollisionDynamic())
dGeomSetBody (m_geoms[i], bodies[i]);
else
dGeomSetBody (m_geoms[i], 0);
}
dSpaceCollide (m_spaceID,this,&nearCallbackStatic);
dWorldQuickStep (m_worldID, steps);
dJointGroupEmpty (m_jointGroup);
for( int i= 0; i<m_shapeCount; i++) {
//air resistance
if(m_shapes[i]->isCollisionDynamic()||m_shapes[i]->isProxyDynamic()) {
const dReal *vel;
const dReal *angvel;
vel= dBodyGetLinearVel (bodies[i]);
dBodyAddForce (bodies[i], -vel[0]*m_airResistance, -vel[1]*m_airResistance, -vel[2]*m_airResistance);
angvel= dBodyGetAngularVel (bodies[i]);
dBodyAddTorque (bodies[i], -angvel[0]*m_airResistance, -angvel[1]*m_airResistance, -angvel[2]*m_airResistance);
}
HHLRC rc= hlGetCurrentContext();
// proxy0
hlMakeCurrent(m_context);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
if(((CProtoHapticApp*)AfxGetApp())->isTwoDevices()) {
// proxy1
hlMakeCurrent(m_context_1);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching1()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
}
hlMakeCurrent(rc);
// gravity
if(m_shapes[i]->isCollisionDynamic())
dBodyAddForce(bodies[i], 0.0, -m_shapes[i]->getMass()*(m_gravity/10.0f), 0.0);
const dReal *pos;
const dReal *rot;
pos= dBodyGetPosition (bodies[i]);
rot= dBodyGetRotation (bodies[i]);
float rotation[16];
rotation[0]= rot[0]; rotation[4]= rot[1]; rotation[8]= rot[2]; rotation[12]= rot[3];
rotation[1]= rot[4]; rotation[5]= rot[5]; rotation[9]= rot[6]; rotation[13]= rot[7];
rotation[2]= rot[8]; rotation[6]= rot[9]; rotation[10]= rot[10]; rotation[14]= rot[11];
rotation[3]= 0.0; rotation[7]= 0.0; rotation[11]= 0.0; rotation[15]= 1.0;
m_shapes[i]->setRotation(rotation);
m_shapes[i]->setLocation(pos[0], pos[1], pos[2]);
}
m_lastSimStep= clock();
}
void SParts::addForceAtPos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz)
{
dBodyAddForceAtPos(m_odeobj->body(), fx, fy, fz, px, py, pz);
}
void ForceHook::Step(Real dt)
{
dBodyAddForceAtPos(body,f.x,f.y,f.z,worldpt.x,worldpt.y,worldpt.z);
}
void PhysicsBody::addForceAtPos(const Vec3f &v, const Vec3f &p)
{
dBodyAddForceAtPos(_BodyID, v.x(), v.y(), v.z(), p.x(), p.y(), p.z());
}
