void SBoxParts::set(dWorldID w, dSpaceID space) { Size &sz = m_cmpnt.size(); /* const dReal hx = sz.x(); const dReal hy = sz.y(); const dReal hz = sz.z(); */ dReal hx = sz.x(); dReal hy = sz.y(); dReal hz = sz.z(); // konao DUMP(("[SBoxParts::set] ODE geom created (hx, hy, hz)=(%f, %f, %f) [%s:%d]\n", hx, hy, hz, __FILE__, __LINE__)); if (hz == 0) hz = 0.001; if (hy == 0) hy = 0.001; if (hx == 0) hx = 0.001; dGeomID geom = dCreateBox(0, hx, hy, hz); m_odeobj = ODEObjectContainer::getInstance()->createODEObj ( w, geom, 0.9, 0.01, 0.5, 0.5, 0.8, 0.001, 0.0 ); dBodyID body = m_odeobj->body(); dMass m; dMassSetZero(&m); // x-axis and z-axis is swapped between ODE/SIGVerse dMassSetBox(&m, DENSITY, hz, hy, hx); //TODO: mass of cube should be configurable dMassAdjust(&m, m_mass); dBodySetMass(body, &m); // Gap between ODE shape and body dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // Initial orientation dReal offq[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()}; dGeomSetOffsetQuaternion(geom, offq); //dMassAdjust(&m, 1.0); m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0); dSpaceAdd(space, geom); dBodySetData(body, this); }
AvatarGameObj::AvatarGameObj(const ORE1::ObjType& obj) : GameObj(obj, Sim::gen_sphere_body(80, 0.5)), // TODO Load mass information from the ORE mission description _xrot_delta(0.0), _zrot_delta(0.0), _ypos_delta(0.0), _ylvel_delta(0.0), _xavel_delta(0.0), _zavel_delta(0.0), _norm_coll_steptime(0), _run_coll_steptime(0), _mesh(MeshAnimation::load("mesh-LIBAvatar")), _attached(false), _attached_this_frame(false) { // TODO Load volume information from the ORE mission description _height = 2.25; _coll_rad = 0.25; // Set up a geom for detecting regular collisions get_entity().set_geom( "physical", dCreateCapsule(Sim::get_dyn_space(), _coll_rad, _height - 2*_coll_rad), std::auto_ptr<CollisionHandler>(new AvatarContactHandler(this)) ); // TODO Maybe some missions start off in upright mode? _uprightness = 0.0; // Set up a geom at our feet to detect when we can run on a surface get_entity().set_geom( "sticky_attach", dCreateRay(Sim::get_dyn_space(), RUNNING_MAX_DELTA_Y_POS*2), std::auto_ptr<CollisionHandler>(new StickyAttachmentContactHandler(this)) ); dQuaternion rdq; dQFromAxisAndAngle(rdq, 1, 0, 0, M_PI_2); dGeomSetOffsetQuaternion(get_entity().get_geom("sticky_attach"), rdq); dGeomDisable(get_entity().get_geom("sticky_attach")); update_geom_offsets(); }
void SCylinderParts::set(dWorldID w, dSpaceID space) { double radius = m_cmpnt.radius(); double length = m_cmpnt.length(); // konao //LOG_MSG(("[SCylinderParts::set] ODE geom created (r, l)=(%f, %f) [%s:%d]\n", radius, length, __FILE__, __LINE__)) // TODO: Ideally, cylinder should be constructed here. However, collision detection // between two cylinders could not be realized. So, Capsule is required // by okamoto@tome on 2011-10-12 //dGeomID geom = dCreateCapsule(0, radius, length); dGeomID geom = dCreateCylinder(0, radius, length); m_odeobj = ODEObjectContainer::getInstance()->createODEObj ( w, geom, 0.9, 0.01, 0.5, 0.5, 0.8, 0.001, 0.0 ); dBodyID body = m_odeobj->body(); dMass m; dMassSetZero(&m); //dMassSetCapsule(&m, DENSITY, 1, radius, length); dMassSetCylinder(&m, DENSITY, 1, radius, length); //TODO: mass of the cylinder should be configurable dMassAdjust(&m, m_mass); dBodySetMass(body, &m); dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz); // gap between ODE shape and body // set the long axis as y axis dReal offq[4] = {0.707, 0.707, 0.0, 0.0}; dReal offq2[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()}; dQuaternion qua; dQMultiply2(qua, offq2, offq); dGeomSetOffsetQuaternion(geom, qua); //dGeomSetOffsetQuaternion(geom, offq2); //dReal tmpq[4] = {0.707, 0.0, 0.0, 0.707}; //dGeomSetQuaternion(geom, tmpq); //dBodySetQuaternion(body, tmpq); /*TODO: Consideration is required whether this procedure is needed * Reflection of orientation of the cylinder * dMatrix3 R; * dRFromAxisAndAngle(dMatrix3 R, dReal rx, dReal ry, dReal rz, dReal angle) * dRFromAxisAndAngle(R,x_axis,y_axis,z_axis,angleData); * dBodySetRotation(body,R); // Request of actual rotation */ // Not used, deleted by inamura // real part of the quaternion // double q = cos(angleData/2.0); // imaginary part of the quaternion // double i,j,k; // i = x_axis * sin(angleData/2.0); // j = y_axis * sin(angleData/2.0); // k = z_axis * sin(angleData/2.0); m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0); dSpaceAdd(space, geom); dBodySetData(body, this); }
void PhysicsGeom::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { Inherited::changed(whichField, origin, details); //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & BodyFieldMask) { if(getBody() != NULL) { dGeomSetBody(_GeomID, getBody()->getBodyID()); } else { dGeomSetBody(_GeomID, 0); } } if(whichField & PositionFieldMask) { dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z()); } if(whichField & RotationFieldMask) { dMatrix3 rotation; Vec4f v1 = getRotation()[0]; Vec4f v2 = getRotation()[1]; Vec4f v3 = getRotation()[2]; rotation[0] = v1.x(); rotation[1] = v1.y(); rotation[2] = v1.z(); rotation[3] = 0; rotation[4] = v2.x(); rotation[5] = v2.y(); rotation[6] = v2.z(); rotation[7] = 0; rotation[8] = v3.x(); rotation[9] = v3.y(); rotation[10] = v3.z(); rotation[11] = 0; dGeomSetRotation(_GeomID, rotation); } if(whichField & QuaternionFieldMask) { dQuaternion q; q[0]=getQuaternion().w(); q[1]=getQuaternion().x(); q[2]=getQuaternion().y(); q[3]=getQuaternion().z(); dGeomSetQuaternion(_GeomID,q); } if(whichField & OffsetPositionFieldMask && getBody() != NULL) { dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z()); } if(whichField & OffsetRotationFieldMask && getBody() != NULL) { dMatrix3 rotation; Vec4f v1 = getOffsetRotation()[0]; Vec4f v2 = getOffsetRotation()[1]; Vec4f v3 = getOffsetRotation()[2]; rotation[0] = v1.x(); rotation[1] = v1.y(); rotation[2] = v1.z(); rotation[3] = 0; rotation[4] = v2.x(); rotation[5] = v2.y(); rotation[6] = v2.z(); rotation[7] = 0; rotation[8] = v3.x(); rotation[9] = v3.y(); rotation[10] = v3.z(); rotation[11] = 0; dGeomSetOffsetRotation(_GeomID, rotation); } if(whichField & OffsetQuaternionFieldMask && getBody() != NULL) { dQuaternion q; q[0]=getOffsetQuaternion().w(); q[1]=getOffsetQuaternion().x(); q[2]=getOffsetQuaternion().y(); q[3]=getOffsetQuaternion().z(); dGeomSetOffsetQuaternion(_GeomID,q); } if(whichField & CategoryBitsFieldMask) { dGeomSetCategoryBits(_GeomID, getCategoryBits()); } if(whichField & CollideBitsFieldMask) { dGeomSetCollideBits(_GeomID, getCollideBits()); } if(whichField & SpaceFieldMask) { dSpaceID CurSpace(dGeomGetSpace(_GeomID)); if(CurSpace != 0 && (getSpace() == NULL || CurSpace != getSpace()->getSpaceID())) { dSpaceRemove(CurSpace,_GeomID); } if(getSpace() != NULL) { dSpaceAdd(getSpace()->getSpaceID(), _GeomID); } } if(whichField & EnableFieldMask) { if(getEnable()) { dGeomEnable(_GeomID); } else { dGeomDisable(_GeomID); } } }