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