GameObject::GameObject(GameWorld& gw, const ObjectPrototype& proto, double x, double y, double z, int id) :
m_meshName(proto.m_meshName), m_sceneEntity(nullptr), m_sceneNode(nullptr), m_lockRotation(proto.m_lockRotation), m_isKinematic(proto.m_isKinematic),
m_maxTurn(proto.m_maxTurnAngle), m_maxForward(proto.m_maxForward), m_maxBackward(proto.m_maxBackward),
m_hitPoints(proto.m_maxHitPoints), m_collisionAccum(0), m_totalDamage(0), m_hasAgent(proto.m_hasAgent), m_gw(gw), m_id(id), m_oldGS(nullptr), m_newGS(nullptr)
{
m_sceneEntity = gw.GetScene()->createEntity(gw.GetMesh(m_meshName));
m_sceneNode = gw.GetScene()->getRootSceneNode()->createChildSceneNode();
m_sceneNode->attachObject(m_sceneEntity);
Ogre::Vector3 size = m_sceneEntity->getBoundingBox().getSize();
m_body = dBodyCreate(gw.GetPhysicsWorld());
m_geom = dCreateBox(gw.GetPhysicsSpace(), size.x, size.y, size.z);
dMassSetBox(&m_mass, proto.m_density, size.x, size.y, size.z);
dBodySetMass(m_body, &m_mass);
dGeomSetBody(m_geom, m_body);
dBodySetPosition(m_body, x, y, z);
// automagically disable things when the body is still for long enough
dBodySetAutoDisableFlag(m_body, 1);
dBodySetAutoDisableLinearThreshold(m_body, 0.1f);
dBodySetAutoDisableAngularThreshold(m_body, 0.25f);
dBodySetAutoDisableSteps(m_body, 1);
// improve simulation accuracy
dBodySetDamping(m_body, 0.0f, 0.1f);
if (proto.m_registerCollisions) {
gw.RegisterForCollisions(this);
}
if (proto.m_isKinematic) {
dBodySetKinematic(m_body);
}
}
void PhysicsActor::postLoad(){
dMass m;
if (type==CUBESHAPE)
{
geom = dCreateBox(space,shape.x,shape.y,shape.z);
dMassSetBox(&m,1.0f,shape.x,shape.y,shape.z);
}
if (type==CAPSULESHAPE)
{
geom = dCreateCapsule(space,shape.x,shape.y);
dMassSetCapsule(&m, shape.z, 3, shape.x, shape.y); //the '3' means align on z-axis and density is shape.z
generateCapsuleList();
}
dMassAdjust(&m,mass);
dBodySetMass(body,&m);
dGeomSetBody(geom,body);
//initialise position
if (base){
Matrix4f bGlobal= base->baseMatrix * renderer->inverseCameraMatrix;
dBodySetPosition(body,bGlobal.data[12] + originalMatrix.data[12] + transformMatrix.data[12],bGlobal.data[13] + originalMatrix.data[13] + transformMatrix.data[13],bGlobal.data[14] + originalMatrix.data[14] + transformMatrix.data[14]);
}else{
dBodySetPosition(body,originalMatrix.data[12] + transformMatrix.data[12],originalMatrix.data[13] + transformMatrix.data[13],originalMatrix.data[14] + transformMatrix.data[14]);
}
dBodySetDamping(body, linearDamp, angleDamp);
bInit=true;
}
void odCable::setupBody(dWorldID *world, dSpaceID space){
int i;
double pi=4.*atan(1.);
double segLen, dL, x;
double dd, s, rs;
dVector3 OP;
odPoint loc1, loc2, d, P, c;
dMass *m;
dGeomID geometry[nSegments];
// dQuaternion align;
dReal align[4];
printf("Setting up cable model\n");
if (body1!=NULL){
loc1=body1->globalPosition(pos1);
}else{
loc1=pos1;
}
if (body2!=NULL){
loc2=body2->globalPosition(pos2);
}else{
loc2=pos2;
}
d=loc2-loc1;
dL=d.mag()/(double)nSegments;
segLen=length/(double)nSegments;
printf("Distance = %lf m\n",d.mag());
printf("Segment length = %lf m\n",segLen);
printf("Allocating memory\n");
element = new dBodyID[nSegments];
joint = new dJointID[nSegments-1];
printf("Creating elements\n");
for (i=0;i<nSegments;i++){
element[i] = dBodyCreate (*world);
dBodySetDamping(element[i], linearDamping, angularDamping);
m=new dMass;
dMassSetBoxTotal(m, weight*segLen, segLen, diameter, diameter);
dMassAdjust (m, weight*segLen);
dBodySetMass (element[i], m);
x=((double)i+.5)*segLen;
dBodySetPosition(element[i], x, 0, 0);
// geometry[i] = dCreateBox(space, length, diameter, diameter);
// dGeomSetBody(geometry[i], element[i]);
}
printf("Creating internal joints\n");
for (i=0;i<nSegments-1;i++){
joint[i] = dJointCreateBall(*world,0);
dJointAttach (joint[i],element[i],element[i+1]);
x = ((double)i+1.)*segLen;
dJointSetBallAnchor (joint[i],x,0,0);
}
/*
c = d.crossProduct(odPoint(0,0,1));
dd = d.dotProduct_natural(odPoint(0,0,1));
s = sqrt((1.0 + dd)*2.0);
rs = 1.0/s;
align[0]=s*0.5; align[1]=c.x*rs; align[2]=c.y*rs; align[3]=c.z*rs;
printf("Moving segments to world positions...\n");
for (i=0;i<nSegments;i++){
P=loc1+d*((double)i)/(nSegments-1);
if (i==0) P.x+=segLen/2.;
if (i==nSegments-1) P.x-=segLen/2.;
dBodySetPosition(element[i], P.x, P.y, P.z);
// dBodySetQuaternion(element[i], align);
}
*/
printf("Creating end joint 1\n");
end1 = dJointCreateBall(*world,0);
if (body1!=NULL){
dJointAttach(end1, body1->odeBody, element[0]);
}else{
dJointAttach(end1, 0, element[0]);
}
// dJointSetBallAnchor (end1, loc1.x, loc1.y, loc1.z);
dJointSetBallAnchor (end1, 0, 0, 0);
end1Feedback = new dJointFeedback;
dJointSetFeedback (end1, end1Feedback);
/*
printf("Creating end joint 2\n");
end2 = dJointCreateBall(*world,0);
if (body2!=NULL){
dJointAttach(end2, body2->odeBody, element[nSegments-1]);
}else{
dJointAttach(end2, 0, element[nSegments-1]);
}
// dJointSetBallAnchor (end2, loc2.x, loc2.y, loc2.z);
dJointSetBallAnchor (end2, nSegments*segLen, 0, 0);
*/
end2Feedback = new dJointFeedback;
dJointSetFeedback (joint[0], end2Feedback);
}
//! add ODE parts with fixed joint
void SSimEntity::setMass(double mass)
{
m_parts.mass = mass;
int geomNum = getGeomNum();
if (geomNum != 0) {
// if a part is already added
// mass per part
double ms = mass / geomNum;
// refer geometry and body
dBodyID body = m_parts.body;
dMass m;
dMass m2;
dMassSetZero(&m); dMassSetZero(&m2);
for (int i = 0; i < geomNum; i++) {
// refer the type of geometory
dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
dReal radius = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&m2, ms, radius);
}
// box
else if (type == 1) {
dVector3 size;
dGeomBoxGetLengths(geom, size);
dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
}
// cylinder
else if (type == 3) {
dReal radius = 0.0;
dReal length = 0.0;
dGeomCylinderGetParams(geom, &radius, &length);
// TODO: confirm: Is 2 suitable for long axis?
dMassSetCylinderTotal(&m2, ms, 2, radius, length);
}
const dReal *pos = dGeomGetPosition(geom);
dMassTranslate(&m2, pos[0], pos[1], pos[2]);
dMassAdd(&m, &m2);
}
/*
for (int i = 0; i < geomNum; i++) {
dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]);
const dReal *pos = dGeomGetPosition(geom);
// Change to a relative position from CoG
dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]);
}
// keep the CoG position
//m_parts.pos.set(m.c[0], m.c[1], m.c[2]);
*/
// Adjustment of the gap from CoG
//const dReal *p = dBodyGetPosition(body);
//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
// Setting of mass
dBodySetMass(body, &m);
dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
} // if (partsNum != 0) {
}
//! setting of mass
void SSimRobotEntity::setMass(SSimObjParts *parts, double mass)
{
// if a part is already added
int geomNum = parts->geoms.size();
if (geomNum != 0) {
// mass per each part
double ms = mass / geomNum;
// refer geometry and body
dBodyID body = parts->body;
dMass m;
dMass m2;
dMassSetZero(&m);
dMassSetZero(&m2);
for (int i = 0; i < geomNum; i++) {
// Refer the type of geometry
dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]);
int type = dGeomGetClass(geom);
// setting of mass
// sphere
if (type == 0) {
dReal radius = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&m2, ms, radius);
}
// box
else if (type == 1) {
dVector3 size;
dGeomBoxGetLengths(geom, size);
dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]);
}
// cylinder
else if (type == 3) {
dReal radius = 0.0;
dReal length = 0.0;
dGeomCylinderGetParams(geom, &radius, &length);
// TODO: confirm: Is 2 suitable for long axis?
dMassSetCylinderTotal(&m2, ms, 2, radius, length);
}
//
const dReal *pos = dGeomGetPosition(geom);
//LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2]));
dMassTranslate(&m2, pos[0], pos[1], pos[2]);
dMassAdd(&m, &m2);
}
// adjustment of the gap between CoG
//const dReal *p = dBodyGetPosition(body);
//dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]);
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
// seeting of mass
dBodySetMass(body, &m);
dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test
} // if (partsNum != 0) {
}
