bool Joint::getJointForces(JointForce *jfs) { dJointFeedback *pfb = dJointGetFeedback(m_joint); if (pfb != NULL && F_SCHOLAR(pfb->f1) > 1.0) { int idx = 0; JointForce *jf = &jfs[idx]; SParts *parts = (SParts*)dBodyGetData(m_bodies[idx]); if (!parts) { return false; } jf->set(parts->name(), Vector3d(pfb->f1[0], pfb->f1[1], pfb->f1[2]), Vector3d(pfb->t1[0], pfb->t1[1], pfb->t1[2])); idx++; jf = &jfs[idx]; parts = (SParts*)dBodyGetData(m_bodies[idx]); if (!parts) { return false; } jf->set(parts->name(), Vector3d(pfb->f2[0], pfb->f2[1], pfb->f2[2]), Vector3d(pfb->t2[0], pfb->t2[1], pfb->t2[2])); return true; } else { return false; } }
void Joint::build(const Vector3d &v, const Rotation &r, bool dynamics) { if (m_bodyNum != BODY_NUM) { return; } assert(m_world); m_joint = createJoint(m_bodies[0], m_bodies[1]); Parts *parts = (Parts*) dBodyGetData(m_bodies[1]); assert(parts); double x, y, z; parts->givePosition(x, y, z); m_rotv.set(x, y, z); m_rotv -= m_anchor; Vector3d av = m_anchor; av.rotate(r); av += v; applyAnchor(av.x(), av.y(), av.z()); if (m_fixed) { dJointSetFixed(m_joint); } if (dynamics) { m_jfb = new dJointFeedback; dJointSetFeedback(m_joint, m_jfb); } }
SParts * Joint::getParts(int idx) { if (idx < m_bodyNum) { dBodyID b = m_bodies[idx]; return (SParts*)dBodyGetData(b); } else { return NULL; } }
IoObject *IoODEBody_bodyFromId(void *state, dBodyID id) { if (id == 0) { return ((IoState*)state)->ioNil; } else { return (IoODEBody*)dBodyGetData(id); } }
IC void add_contact_body_effector(dBodyID body,const dContact& c,SGameMtl* material) { CPHContactBodyEffector* effector=(CPHContactBodyEffector*)dBodyGetData(body); if(effector) effector->Merge(c,material); else { effector=ContactEffectors.add(); effector->Init(body,c,material); dBodySetData(body,(void*)effector); } }
static void staticCollisionCallback(Callback* callback, dGeomID geom1, dGeomID geom2) { ASSERT(!dGeomIsSpace(geom1)); ASSERT(!dGeomIsSpace(geom2)); ASSERT(dGeomGetBody(geom1) || dGeomGetBody(geom2)); dContact contact[1]; if(dCollide(geom1, geom2, 1, &contact[0].geom, sizeof(dContact)) < 1) return; dGeomID geom = geom2; dBodyID bodyId = dGeomGetBody(geom2); if(!bodyId) { bodyId = dGeomGetBody(geom1); geom = geom1; } const dReal* pos = dGeomGetPosition(geom); float sqrDistance = (Vector3<>((float) pos[0], (float) pos[1], (float) pos[2]) - callback->cameraPos).squareAbs(); if(!callback->closestBody || sqrDistance < callback->closestSqrDistance) { callback->closestBody = (Body*)dBodyGetData(bodyId); callback->closestSqrDistance = sqrDistance; } }
static void nearCallback (void *data, dGeomID o1, dGeomID o2) { //make sure both of the geom's are enabled (otherwise this is //a waste of our time if( !dGeomIsEnabled( o1 ) || !dGeomIsEnabled (o2 )) { return; } //collide spaces if neccesary if (dGeomIsSpace (o1) || dGeomIsSpace (o2)) { // collide the space(s?) dSpaceCollide2 (o1,o2,data,&nearCallback); // collide all geoms/spaces within the spaces //if (dGeomIsSpace (o1)) dSpaceCollide ((dSpaceID)o1,data,&nearCallback); //if (dGeomIsSpace (o2)) dSpaceCollide ((dSpaceID)o2,data,&nearCallback); } //otherwise no spaces, just collide geoms else { //make sure one of the geoms has a body dBodyID body1 = dGeomGetBody( o1 ); dBodyID body2 = dGeomGetBody( o2 ); //if( body1 == 0 && body2 == 0) //return; //make sure that the bodies are enabled //declarations DynamicsObject* dynobj=NULL, *dynobj2=NULL; int n; DynamicsSolver* solver = (DynamicsSolver*)data; //Get 64 contacts const int N = 64; dContact contact[N]; n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact)); //Sort all the contacts dContact* sortedContacts[N]; for( int i=0; i<n; i++ ) sortedContacts[i] = &contact[i]; qsort( sortedContacts, n,sizeof( dContact*), compareContacts ); //determine how many contacts to actually care about //if( n > 8 ) // n = 8; if (n > 0) { //figure out surface desc stuff DynamicsSurfaceDesc* sd1, *sd2; DynamicsSurfaceDesc finaldesc; sd1 = (DynamicsSurfaceDesc*)dGeomGetData(contact[0].geom.g1); sd2 = (DynamicsSurfaceDesc*)dGeomGetData(contact[0].geom.g2); finaldesc.Combine( sd1, sd2); //Get the bodies involved in the collision dBodyID b1, b2; b1 = dGeomGetBody(contact[0].geom.g1); b2 = dGeomGetBody(contact[0].geom.g2); //Inform objects of the collision that occured. if(b1) dynobj = (DynamicsObject*)dBodyGetData(b1); if(b2) dynobj2 = (DynamicsObject*)dBodyGetData(b2); if( dynobj ) dynobj->OnCollide ( dynobj2 ); if( dynobj2) dynobj2->OnCollide( dynobj); //Generate contact joints for (int i=0; i<n; i++) { contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;// | dContactBounce; //contact[i].surface.mu = 0.5f; contact[i].surface.mu = finaldesc.mu; if(contact[i].geom.normal[1] < .8f ) { contact[i].surface.mu = 0.1f; } contact[i].surface.slip1 = finaldesc.ContactSlip1 ; contact[i].surface.slip2 = finaldesc.ContactSlip2 ; contact[i].surface.soft_erp = finaldesc.SoftERP ; contact[i].surface.soft_cfm = finaldesc.SoftCFM; contact[i].surface.bounce = finaldesc.Bounce; contact[i].surface.bounce_vel = finaldesc.BounceVelocity ; dJointID c = dJointCreateContact (solver->WorldID,solver->ContactGroup,&contact[i]); //Insert friction anisotropy code here fixFrictionVector( b1, b2, contact[i] ); dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2)); } } } }
// The "Near Callback". ODE calls this during collision detection to // decide whether 2 geoms collide, and if yes, to generate Contact // joints between the 2 involved rigid bodies. void CLevel::ODENearCallback(void* data, dGeomID o1, dGeomID o2) { CLevel* Level = (CLevel*)data; Level->statsNumNearCallbackCalled++; // handle sub-spaces if (dGeomIsSpace(o1) || dGeomIsSpace(o2)) { // collide a space with something Level->statsNumSpaceCollideCalled++; dSpaceCollide2(o1, o2, data, &ODENearCallback); return; } // handle shape/shape collisions dBodyID Body1 = dGeomGetBody(o1); dBodyID Body2 = dGeomGetBody(o2); n_assert(Body1 != Body2); // do nothing if 2 bodies are connected by a joint if (Body1 && Body2 && dAreConnectedExcluding(Body1, Body2, dJointTypeContact)) { // FIXME: bodies are connected, check if jointed-collision is enabled // for both bodies (whether 2 bodies connected by a joint should // collide or not, for this, both bodies must have set the // CollideConnected() flag set. CRigidBody* PhysicsBody0 = (CRigidBody*)dBodyGetData(Body1); n_assert(PhysicsBody0 && PhysicsBody0->IsInstanceOf(CRigidBody::RTTI)); if (!PhysicsBody0->CollideConnected) return; CRigidBody* PhysicsBody1 = (CRigidBody*) dBodyGetData(Body2); n_assert(PhysicsBody1 && PhysicsBody1->IsInstanceOf(CRigidBody::RTTI)); if (!PhysicsBody1->CollideConnected) return; } CShape* Shape1 = CShape::GetShapeFromGeom(o1); CShape* Shape2 = CShape::GetShapeFromGeom(o2); n_assert(Shape1 && Shape2); n_assert(!((Shape1->GetType() == CShape::Mesh) && (Shape2->GetType() == CShape::Mesh))); Level->statsNumCollideCalled++; // initialize Contact array bool MaterialsValid = (Shape1->GetMaterialType() != InvalidMaterial && Shape2->GetMaterialType() != InvalidMaterial); float Friction; float Bounce; if (MaterialsValid) { Friction = Physics::CMaterialTable::GetFriction(Shape1->GetMaterialType(), Shape2->GetMaterialType()); Bounce = Physics::CMaterialTable::GetBounce(Shape1->GetMaterialType(), Shape2->GetMaterialType()); } else { Friction = 0.f; Bounce = 0.f; } static dContact Contact[MaxContacts]; for (int i = 0; i < MaxContacts; i++) { Contact[i].surface.mode = dContactBounce | dContactSoftCFM; Contact[i].surface.mu = Friction; Contact[i].surface.mu2 = 0.0f; Contact[i].surface.bounce = Bounce; Contact[i].surface.bounce_vel = 1.0f; Contact[i].surface.soft_cfm = 0.0001f; Contact[i].surface.soft_erp = 0.2f; } // do collision detection int CollisionCount = dCollide(o1, o2, MaxContacts, &(Contact[0].geom), sizeof(dContact)); //???!!!set in OnCollision?! Shape1->SetNumCollisions(Shape1->GetNumCollisions() + CollisionCount); Shape2->SetNumCollisions(Shape2->GetNumCollisions() + CollisionCount); if (CollisionCount > 0) { Level->statsNumCollided++; if (!Shape1->OnCollide(Shape2) || !Shape2->OnCollide(Shape1)) return; // create a Contact for each collision for (int i = 0; i < CollisionCount; i++) dJointAttach( dJointCreateContact(Level->ODEWorldID, Level->ContactJointGroup, Contact + i), Body1, Body2); } //???sounds here or in sound system on event? // FIXME: not really ready for prime Time // TODO: implement roll / slide sounds (sounds that stop as soon as the Contact is gone) // roll / slide sounds also need to consider relative velocity nTime Now = GameSrv->GetTime(); if (CollisionCount != 0) { CShape* Key[2]; // build an unique Key for every colliding shape combination if (Shape1 < Shape2) { Key[0] = Shape1; Key[1] = Shape2; } else { Key[0] = Shape2; Key[1] = Shape1; } if ((Now - Level->CollisionSounds.At(Key, sizeof(Key))) > 0.25f) { CRigidBody* Rigid1 = Shape1->GetRigidBody(); CRigidBody* Rigid2 = Shape2->GetRigidBody(); if ((!Rigid1 || !Rigid1->IsEnabled()) && (!Rigid2 || !Rigid2->IsEnabled())) return; nString Sound; if (MaterialsValid) Physics::CMaterialTable::GetCollisionSound(Shape1->GetMaterialType(), Shape2->GetMaterialType()); if (Sound.IsValid()) { vector3 Normal; CPhysicsServer::OdeToVector3(Contact[0].geom.normal, Normal); vector3 Velocity = Rigid1 ? Rigid1->GetLinearVelocity() : vector3(0.0f, 0.0f, 0.0f); if (Rigid2) Velocity -= Rigid2->GetLinearVelocity(); float Volume = n_saturate((-Velocity.dot(Normal) - 0.3f) / 4.0f); if (Volume > 0.0f) { Ptr<Event::PlaySound> Evt = Event::PlaySound::Create(); Evt->Name = Sound; Evt->Position.set(Contact[0].geom.pos[0], Contact[0].geom.pos[1], Contact[0].geom.pos[2]); Evt->Volume = Volume; EventMgr->FireEvent(*Evt); Level->CollisionSounds.At(Key, sizeof(Key)) = Now; } } } } }
static void nearCB(void *data, dGeomID o1, dGeomID o2) { ODEObj *odeobj1 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o1); ODEObj *odeobj2 = ODEObjectContainer::getInstance()->getODEObjFromGeomID(o2); /* SSimRobotEntity *ent1 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o1); SSimRobotEntity *ent2 = ODEObjectContainer::getInstance()->getSSimRobotEntityFromGeomID(o2); if(ent1 != NULL && ent2 != NULL && ent1->name() == ent2->name()){ //LOG_MSG(("name (%s, %s)",ent1->name().c_str(), ent2->name().c_str())); return; } */ SParts *p1 = NULL; SParts *p2 = NULL; dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); if (b1 == b2) { return; } ODEWorld *world = ODEWorld::get(); dGeomID ground = world->getGround(); if (b1 && b2) { if (dAreConnected(b1, b2)) { return; } } if (b1) { void *d = dBodyGetData(b1); if (d) { p1 = (SParts*)d; if (p1->isBlind()) { return; } } } if (b2) { void *d = dBodyGetData(b2); if (d) { p2 = (SParts*)d; if (p2->isBlind()) { return; } } } if (p1 && p2 && p1->sameParent(*p2)) { return; } if (dGeomIsSpace(o1) && dGeomIsSpace(o2)) { dSpaceCollide2(o1, o2, data, &collideCB); return; } #define F_SCHOLAR(V) sqrt(V[0]*V[0] + V[1]*V[1] + V[2]*V[2]) static dJointFeedback fb; #define MAX_COLLISION 32 const int N = MAX_COLLISION; dContact contacts[N]; int n = dCollide(o1, o2, N, &(contacts[0].geom), sizeof(contacts[0])); if (n > 0) { ODEWorld *world = ODEWorld::get(); for (int i=0; i<n; i++) { dContact *c = &contacts[i]; dContactGeom &g = c->geom; if (p1 && p2) { #if 0 LOG_SYS(("Collision #%d/%d %s(geomID=%d) <-> %s(geomID=%d)", i, n, p1->getParent()->name(), o1, p2->getParent()->name(), o2)); LOG_SYS(("\tpos = (%f, %f, %f)", g.pos[0], g.pos[1], g.pos[2])); LOG_SYS(("\tnormal = (%f, %f, %f)", g.normal[0], g.normal[1], g.normal[2])); LOG_SYS(("\tfdir = (%f, %f, %f)", c->fdir1[0], c->fdir1[1], c->fdir1[2])); LOG_SYS(("\tdepth = %f", g.depth)); #endif const char *name1 = p1->getParent()->name(); const char *name2 = p2->getParent()->name(); std::string key = chash_key(name1, name2); if (key.length() <= 0) { continue; } if (chash_find(key)) { continue; } s_chash[key] = true; s_collisions.push_back(ODEWorld::Collision(name1, name2, p1->name(), p2->name())); // Set the collision flag to be ON p1->setOnCollision(true); p2->setOnCollision(true); } //c->surface.mode = dContactBounce; c->surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce; //c->surface.mode = dContactSlip1 | dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactBounce; // // Reflection of material parameters of the collided object // Fliction force should be regarded as average of contiguous material (???) // TODO: Calclation of fliction force sould be considered // #if 0 if (odeobj1 && odeobj2) { c->surface.mu = ( odeobj1->getMu1() + odeobj2->getMu1() ) / 2.0; c->surface.mu2 = ( odeobj1->getMu2() + odeobj2->getMu2() ) / 2.0; c->surface.slip1 = ( odeobj1->getSlip1() + odeobj2->getSlip1() ) / 2.0; c->surface.slip2 = ( odeobj1->getSlip2() + odeobj2->getSlip2() ) / 2.0; c->surface.soft_erp = ( odeobj1->getSoftErp() + odeobj2->getSoftErp() ) / 2.0; c->surface.soft_cfm = ( odeobj1->getSoftCfm() + odeobj2->getSoftCfm() ) / 2.0; c->surface.bounce = ( odeobj1->getBounce() + odeobj2->getBounce() ) / 2.0; } else { c->surface.bounce_vel = world->getCollisionParam("bounce_vel"); c->surface.bounce = world->getCollisionParam("bounce"); c->surface.mu = world->getCollisionParam("mu"); c->surface.mu2 = world->getCollisionParam("mu2"); c->surface.slip1 = world->getCollisionParam("slip1"); c->surface.slip2 = world->getCollisionParam("slip2"); c->surface.soft_erp = world->getCollisionParam("soft_erp"); c->surface.soft_cfm = world->getCollisionParam("soft_cfm"); } #else c->surface.mu = SPARTS_MU1; c->surface.mu2 = SPARTS_MU2; c->surface.slip1 = SPARTS_SLIP1; c->surface.slip2 = SPARTS_SLIP2; c->surface.soft_erp = SPARTS_ERP; // parameter for modify the error of Joint position (0..1); if it is 1, modification will be perfect c->surface.soft_cfm = SPARTS_CFM; // If this value=0, joint velocity is strange c->surface.bounce = SPARTS_BOUNCE; // is a little smaller than ball c->surface.bounce_vel = 0.0; #endif dJointID cj = dJointCreateContact(world->world(), world->jointGroup(), c); //dJointAttach(cj, dGeomGetBody(o1), dGeomGetBody(o2)); //by MSI dJointAttach(cj, dGeomGetBody(c->geom.g1), dGeomGetBody(c->geom.g2)); // by Demura.net #if 0 if (p1 && p2) { dJointSetFeedback(cj, &fb); dJointFeedback *pfb = dJointGetFeedback(cj); if (F_SCHOLAR(pfb->f1) > 1.0) { LOG_SYS(("\tF1 = (%f, %f, %f)", pfb->f1[0], pfb->f1[1], pfb->f1[2])); LOG_SYS(("\tF2 = (%f, %f, %f)\n", pfb->f2[0], pfb->f2[1], pfb->f2[2])); } } #endif } } }
void Simulation::nearCallback(void* data, dGeomID o1, dGeomID o2) { int i; // if (o1->body && o2->body) return; // exit without doing anything if the two bodies are connected by a joint dBodyID b1 = dGeomGetBody(o1); dBodyID b2 = dGeomGetBody(o2); //PEXP(b1); //PEXP(b2); //if (dAreConnected (b1, b2)) cout << "debug: CONNECTED CONNECTED OVER\n"; if (b1 && b2 && dAreConnected(b1, b2)) return; int g1 = (o1 == this->ground); int g2 = (o2 == this->ground); int not_ground = (!(g1 ^ g2)); int one_is_link = (o1 != this->ground) && (b1) && ( dBodyGetData(b1) == (void*)tagLink); int two_is_link = (o2 != this->ground) && (b2) && ( dBodyGetData(b2) == (void*)tagLink); int link_link = one_is_link && two_is_link; double mu; // determine friction if (link_link) mu = 0; else if (not_ground) mu = this->mu_non_ground; else mu = this->mu_ground; // profiling this->callback_count++; // debug //if (b1 == sprocket || b2 == sprocket) { cout << "debug: collision with sprocket\n"; } dContact contact[3]; // up to 3 contacts per box if (not_ground) { // except for mu, link_link and link_sprocket are bundled together for (i = 0; i < 3; i++) { // contact[i].surface.mode = dContactSoftCFM | dContactApprox1; contact[i].surface.mode = 0; contact[i].surface.mu = mu; contact[i].surface.soft_cfm = 0.01; } } else { for (i = 0; i < 3; i++) { contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1; // Approx1 means friction only for non zero normal contact[i].surface.mu = mu;//dInfinity; contact[i].surface.slip1 = 0.1; contact[i].surface.slip2 = 0.1; contact[i].surface.soft_erp = this->ground_soft_erp; //0.3; contact[i].surface.soft_cfm = this->ground_soft_cfm; //0.5; } } if (int numc = dCollide(o1, o2, 3, &contact[0].geom, sizeof(dContact))) { // profiling if (not_ground) { this->not_ground_count++; // std::cout << "debug: not_ground collision\n"; } else { this->ground_count++; //cout << "debug: ground collision\n"; // if a link is involved, increment its count of contact joints dBodyID one_link = (o1 == this->ground? b2 : b1); dBodySetData(one_link, (void*)((int)dBodyGetData(one_link) + numc)); }; //dMatrix3 RI; //dRSetIdentity (RI); //const dReal ss[3] = {0.02,0.02,0.02}; //PEXP(numc); for (i = 0; i < numc; i++) { dJointID c = dJointCreateContact(this->world, this->contactgroup, contact + i); dJointAttach(c, b1, b2); // dsDrawBox (contact[i].geom.pos,RI,ss); } } }
void* PhysicsBody::getData(void) { return dBodyGetData(_BodyID); }
void Simulation::staticCollisionCallback(Simulation* simulation, dGeomID geomId1, dGeomID geomId2) { ASSERT(!dGeomIsSpace(geomId1)); ASSERT(!dGeomIsSpace(geomId2)); #ifndef NDEBUG { dBodyID bodyId1 = dGeomGetBody(geomId1); dBodyID bodyId2 = dGeomGetBody(geomId2); ASSERT(bodyId1 || bodyId2); Body* body1 = bodyId1 ? (Body*)dBodyGetData(bodyId1) : 0; Body* body2 = bodyId2 ? (Body*)dBodyGetData(bodyId2) : 0; ASSERT(!body1 || !body2 || body1->rootBody != body2->rootBody); } #endif dContact contact[32]; int collisions = dCollide(geomId1, geomId2, 32, &contact[0].geom, sizeof(dContact)); if(collisions <= 0) return; Geometry* geometry1 = (Geometry*)dGeomGetData(geomId1); Geometry* geometry2 = (Geometry*)dGeomGetData(geomId2); if(geometry1->collisionCallbacks && !geometry2->immaterial) { for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry1->collisionCallbacks->begin(), end = geometry1->collisionCallbacks->end(); i != end; ++i) (*i)->collided(*geometry1, *geometry2); if(geometry1->immaterial) return; } if(geometry2->collisionCallbacks && !geometry1->immaterial) { for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry2->collisionCallbacks->begin(), end = geometry2->collisionCallbacks->end(); i != end; ++i) (*i)->collided(*geometry2, *geometry1); if(geometry2->immaterial) return; } dBodyID bodyId1 = dGeomGetBody(geomId1); dBodyID bodyId2 = dGeomGetBody(geomId2); ASSERT(bodyId1 || bodyId2); float friction = 1.f; if(geometry1->material && geometry2->material) { if(!geometry1->material->getFriction(*geometry2->material, friction)) friction = 1.f; float rollingFriction; if(bodyId1) switch(dGeomGetClass(geomId1)) { case dSphereClass: case dCCylinderClass: case dCylinderClass: if(geometry1->material->getRollingFriction(*geometry2->material, rollingFriction)) { dBodySetAngularDamping(bodyId1, 0.2); Vector3<> linearVel; ODETools::convertVector(dBodyGetLinearVel(bodyId1), linearVel); linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength)); dBodySetLinearVel(bodyId1, linearVel.x, linearVel.y, linearVel.z); } break; } if(bodyId2) switch(dGeomGetClass(geomId2)) { case dSphereClass: case dCCylinderClass: case dCylinderClass: if(geometry2->material->getRollingFriction(*geometry1->material, rollingFriction)) { dBodySetAngularDamping(bodyId2, 0.2); Vector3<> linearVel; ODETools::convertVector(dBodyGetLinearVel(bodyId2), linearVel); linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength)); dBodySetLinearVel(bodyId2, linearVel.x, linearVel.y, linearVel.z); } break; } } for(dContact* cont = contact, * end = contact + collisions; cont < end; ++cont) { cont->surface.mode = simulation->scene->contactMode | dContactApprox1; cont->surface.mu = friction; /* cont->surface.bounce = 0.f; cont->surface.bounce_vel = 0.001f; cont->surface.slip1 = 0.f; cont->surface.slip2 = 0.f; */ cont->surface.soft_erp = simulation->scene->contactSoftERP; cont->surface.soft_cfm = simulation->scene->contactSoftCFM; dJointID c = dJointCreateContact(simulation->physicalWorld, simulation->contactGroup, cont); ASSERT(bodyId1 == dGeomGetBody(cont->geom.g1)); ASSERT(bodyId2 == dGeomGetBody(cont->geom.g2)); dJointAttach(c, bodyId1, bodyId2); } ++simulation->collisions; simulation->contactPoints += collisions; }
void base::active_square_render (void) { //with frame-animation, but without set-type img //0 alive, 1 - dead, nocontrol, 2 - delete int *gb; if (!bitd){ gb = (int*) dBodyGetData (body); if (gb <= (int*) 1900 && gb != (int*) 0 ){ printf("DEAD\n"); if(!bitd) bitd=1; } } else { last=DL; translated_val=0; } if(bitd==1 && !up){ dBodyDestroy(body); dGeomDestroy(geom); printf("DESTROY\n"); bitd=2; } if(bitd!=2){ if(body) odepos=dBodyGetPosition(body); else if(geom) odepos=dGeomGetPosition(geom); x=odepos[0]-texture[last].w/2; //is it needed? y=odepos[1]-texture[last].h/2; //here maybe some troubles with up } glMatrixMode(GL_TEXTURE); glLoadIdentity(); glTranslated(translated_val, 0, 0); //for all except stand anim glScalef((float)1/texture[last].n, 1.0f, 1.0f); //1==fullsize. -(w+1/n)==one frame size + -- invers /*invers--------------------------- glTranslated(-translated_val, 0, 0); glScalef(-(float)1/texture[last].n, 1.0f, 1.0f); invers----------------------------- */ glBindTexture( GL_TEXTURE_2D, texture[last].texture ); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); if(rotatebit) rotate(); glTranslated(x, y, 0); //set position glTranslated(texture[last].w/2, texture[last].h/2, 0); //all three for rotate glRotatef(rotate_angle,0,0,1); glTranslated(-texture[last].w/2, -texture[last].h/2, 0); glBegin( GL_QUADS ); glColor3f(1.0f,1.0f,1.0f); //Bottom-r vertex (corner) glTexCoord2i( 0, 1 ); glVertex3f( 0.f, 0.f, 0.0f ); //Bottom-l vertex (corner) glTexCoord2i( 1,1 ); glVertex3f( texture[last].w, 0.f, 0.f ); //Top-l vertex (corner) glTexCoord2i( 1, 0 ); glVertex3f( texture[last].w, texture[last].h, 0.f ); //Top-r vertex (corner) glTexCoord2i( 0,0 ); glVertex3f( 0.f, texture[last].h, 0.f ); glEnd(); if(body && bitd!=2)hack_2d(); }