void CollidableObject::SetCanCollide(bool canCollide)
{
if(canCollide)
dGeomEnable(mGeom);
else
dGeomDisable(mGeom);
}
//! Setting of flag for collision detection
void SSimRobotEntity::setCollision(bool col)
{
if (col)
m_collision = true;
else
m_collision = false;
// Setting all of the parts
int pSize = m_allParts.size();
for (int i = 0; i < pSize; i++) {
int gSize = m_allParts[i].objParts.geoms.size();
for (int j = 0; j < gSize; j++) {
// reference of the geometry
dGeomID geom = m_allParts[i].objParts.geoms[j];
if (col) {
dGeomEnable(geom);
}
else {
dGeomDisable(geom);
}
}
}
}
void SParts::setCollisionEnable(bool gravity)
{
if(gravity)
dGeomEnable(m_odeobj->geom());
else
dGeomDisable(m_odeobj->geom());
}
void EOSOdeCollisionObject::setActive(bool activityStatus)
{
if(this->collisionGeometry != 0)
{
activityStatus ? dGeomEnable(this->collisionGeometry) : dGeomDisable(this->collisionGeometry);
}
}
void YGEBodyAsset::enable(){
if(hasBody) {
dGeomEnable(geomId);
dBodyDisable(bodyId);
}
}
void Construction::EnableAllGeoms()
{
for(int i=0; i<nObjects; i++)
{
if(ObjectList[i].HasGeom )
dGeomEnable( ObjectList[i].Geom );
}
}
void Construction::Activate()
{
for(int i=0; i<nObjects; i++)
{
dBodyEnable( ObjectList[i].Body );
if( ObjectList[i].HasGeom )
dGeomEnable( ObjectList[i].Geom );
}
Active = true;
}
//! Add ODE parts with fixed joint
void SSimEntity::setCollision(bool col)
{
if (col)
m_collision = true;
else
m_collision = false;
// if a part is already added
int geomNum = getGeomNum();
if (geomNum != 0) {
for (int i = 0; i < geomNum; i++) {
// refer the geometry
dGeomID geom = m_parts.geoms[i];
if (col) {
dGeomEnable(geom);
}
else {
dGeomDisable(geom);
}
}
}
}
void Physics::perframe(){
//dWorldStep(worldId);
return; //not implemented yet
#ifdef ODE
for(int i=0; i<level->colliders.size(); i++){
Object* obj=level->colliders[i];
if(level->colliders[i]->collideType=="box" || level->colliders[i]->collideType=="cube"){
if(level->colliders[i]->oldCollideType!="box" && level->colliders[i]->oldCollideType!="cube"){
obj->geomId=dCreateBox(spaceId,obj->collideParameters.x,obj->collideParameters.y,obj->collideParameters.z);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
if(obj->boxBuilt){
float lx=level->colliders[i]->box.px-level->colliders[i]->box.nx;
float ly=level->colliders[i]->box.py-level->colliders[i]->box.ny;
float lz=level->colliders[i]->box.pz-level->colliders[i]->box.nz;
dGeomBoxSetLengths (level->colliders[i]->geomId, lx, ly, lz);
}
}else if(obj->collideType=="sphere"){
if(obj->oldCollideType!="sphere"){
obj->geomId=dCreateSphere(spaceId,obj->collideParameters.x);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
}
}else if(obj->collideType=="plane"){
obj->geomId=dCreatePlane(spaceId,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x,obj->collideParameters.x);
}else if(obj->collideType=="cylindar"){
obj->geomId=dCreateCCylinder(spaceId,obj->collideParameters.x,obj->collideParameters.y);
}else if(obj->collideType=="triangle"){
if(obj->oldCollideType!="triangle"){
dTriMeshDataID tridat=dGeomTriMeshDataCreate();
obj->geomId=dCreateTriMesh (spaceId, tridat,
NULL,
NULL,
NULL);
dGeomSetBody(obj->geomId,obj->bodyId);
obj->geomIdInit=true;
level->colliders[i]->oldCollideType=level->colliders[i]->collideType;
}
}
if(obj->collide){
dGeomEnable(obj->geomId);
}else{
dGeomDisable(obj->geomId);
}
for(int j=0; j<level->colliders.size(); j++){
if(!level->colliders[i]->geomIdInit || !level->colliders[j]->geomIdInit){
continue;
}
dContactGeom contactg[2];
int cnt=dCollide(level->colliders[i]->geomId,level->colliders[j]->geomId,0,contactg,sizeof(dContactGeom));
if(cnt>0){
dSurfaceParameters surf;
surf.mode=0;
surf.mode=dContactBounce;
surf.mu=level->colliders[i]->friction;
surf.bounce=level->colliders[i]->bounce;
surf.bounce_vel=level->colliders[i]->bounceThreshold;
dContact contact;
contact.geom=contactg[0];
contact.surface=surf;
dJointID joint=dJointCreateContact(worldId,0,&contact);
if(!level->colliders[i]->dynamic){
dJointAttach(joint,level->colliders[j]->bodyId,0);
}else if(!level->colliders[j]->dynamic){
dJointAttach(joint,level->colliders[i]->bodyId,0);
}else{
dJointAttach(joint,level->colliders[i]->bodyId,level->colliders[j]->bodyId);
}
}
}
}
dWorldSetGravity(worldId,gravity.x,gravity.y,gravity.z);
dWorldQuickStep(worldId,1);
for(int i=0; i<level->dynamicObjects.size(); i++){
if(!level->dynamicObjects[i]->bodyIdInit){
continue;
}
if(level->dynamicObjects[i]->dynamic){
dBodyEnable(level->dynamicObjects[i]->bodyId);
}else{
dBodyDisable(level->dynamicObjects[i]->bodyId);
}
const dReal* pos=new dReal[3];
pos=dBodyGetPosition(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->pos.x=pos[0];
level->dynamicObjects[i]->pos.y=pos[1];
level->dynamicObjects[i]->pos.z=pos[2];
const dReal* rot=new dReal[3];
rot=dBodyGetRotation(level->dynamicObjects[i]->bodyId);
level->dynamicObjects[i]->rot.x=rot[0];
level->dynamicObjects[i]->rot.y=rot[1];
level->dynamicObjects[i]->rot.z=rot[2];
}
#endif
}
//===========================================================================
void cODEGenericBody::enableDynamics()
{
if (m_ode_body == NULL) { return; }
dBodyEnable(m_ode_body);
dGeomEnable(m_ode_geom);
}
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);
}
}
}
void AvatarGameObj::step_impl() {
dBodyID body = get_entity().get_id();
const Channel* chn;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
dBodyAddRelForce(body, -v, 0.0, 0.0);
}
bool pushing_up = false;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateY);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
if (Saving::get().config().invertTranslateY()) {
v = -v;
}
dBodyAddRelForce(body, 0.0, -v, 0.0);
if (v < 0) {
pushing_up = true;
}
}
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ACCEL/MAX_FPS);
dBodyAddRelForce(body, 0.0, 0.0, -v);
}
const dReal* avel = dBodyGetAngularVel(body);
dVector3 rel_avel;
dBodyVectorFromWorld(body, avel[0], avel[1], avel[2], rel_avel);
// X-turn and x-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateY);
if (chn->is_on()) {
float v = -(chn->get_value())*(MAX_TURN/MAX_FPS);
dBodyAddRelTorque(body, 0.0, v, 0.0);
} else {
float cv = rel_avel[1]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, 0.0, cv, 0.0);
}
// Y-turn and y-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_TURN/MAX_FPS);
if (Saving::get().config().invertRotateY()) {
v = -v;
}
dBodyAddRelTorque(body, v, 0.0, 0.0);
} else {
float cv = rel_avel[0]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, cv, 0.0, 0.0);
}
// Roll and counter-roll
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ROLL/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, v);
} else {
float cv = rel_avel[2]*(-CROLL_COEF/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, cv);
}
// Changing stance between superman-style and upright
if (_attached) {
_uprightness += UPRIGHTNESS_STEP_DIFF;
} else {
_uprightness -= UPRIGHTNESS_STEP_DIFF;
}
if (_uprightness > 1.0) { _uprightness = 1.0; } else if (_uprightness < 0.0) { _uprightness = 0.0; }
update_geom_offsets();
_attached = _attached_this_frame;
// If we are attached, work to keep ourselves ideally oriented to the attachment surface
if (_attached) {
Vector sn_rel = vector_from_world(_sn);
Vector lvel = Vector(dBodyGetLinearVel(body));
Vector lvel_rel = vector_from_world(lvel);
Vector avel = Vector(dBodyGetAngularVel(body));
Vector avel_rel = vector_from_world(avel);
// Apply as much of each delta as we can
// X and Z orientation delta
// TODO Maybe should translate body so that the contact point stays in the same spot through rotation
float a = limit_abs(_zrot_delta, RUNNING_ADJ_RATE_Z_ROT/MAX_FPS);
Vector body_x(vector_to_world(Vector(cos(a), sin(a), 0)));
a = limit_abs(-_xrot_delta, RUNNING_ADJ_RATE_X_ROT/MAX_FPS);
Vector body_y(vector_to_world(Vector(0, cos(a), sin(a))));
dMatrix3 matr;
dRFrom2Axes(matr, body_x.x, body_x.y, body_x.z, body_y.x, body_y.y, body_y.z);
dBodySetRotation(body, matr);
// Y position delta
// If the user is pushing up, set the target point high above the ground so we escape sticky attachment
set_pos(get_pos() + _sn*limit_abs(_ypos_delta + (pushing_up ? RUNNING_MAX_DELTA_Y_POS*2 : 0), RUNNING_ADJ_RATE_Y_POS/MAX_FPS));
// Y linear velocity delta
lvel_rel.y += limit_abs(_ylvel_delta, RUNNING_ADJ_RATE_Y_LVEL/MAX_FPS);
lvel = vector_to_world(lvel_rel);
dBodySetLinearVel(body, lvel.x, lvel.y, lvel.z);
// X and Z angular velocity delta
avel_rel.x += limit_abs(_xavel_delta, RUNNING_ADJ_RATE_X_AVEL/MAX_FPS);
avel_rel.z += limit_abs(_zavel_delta, RUNNING_ADJ_RATE_Z_AVEL/MAX_FPS);
avel = vector_to_world(avel_rel);
dBodySetAngularVel(body, avel.x, avel.y, avel.z);
}
if (_attached_this_frame) {
_attached_this_frame = false;
dGeomEnable(get_entity().get_geom("sticky_attach"));
} else {
dGeomDisable(get_entity().get_geom("sticky_attach"));
}
}
