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