/**
* This method is used for activating or deactivating
* the Joint. If the Joint was inactive and should be
* activated the fixed ODE-joint is removed and the
* virtual attachJoint-method is called.
* If the Joint was active and should be deactivated
* the virtual detachJoint-method is called and then
* a fixed ODE-joint is created and attached to the bodies.
* @param isActive 0 .. not active
* anything else .. active
**/
void Joint::setActive(int isActive)
{
dBodyID body1, body2;
if (!active && isActive && joint)
{
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
attachJoint(body1, body2);
} // if
else if (active && !isActive && joint)
{
// first detach joint e.g. for saving axis angles
detachJoint();
dJointDestroy(joint);
body1 = jointInteraction->getBodyWithID(entityID1);
body2 = jointInteraction->getBodyWithID(entityID2);
joint = dJointCreateFixed(world, 0);
dJointAttach(joint, body1, body2);
dJointSetFixed(joint);
} // else if
if (isActive)
this->active = true;
else
this->active = false;
} // setActive
void TrackedVehicle::destroy() {
this->leftTrack->destroy();
dJointDestroy(this->leftTrackJoint);
this->rightTrack->destroy();
dJointDestroy(this->rightTrackJoint);
dBodyDestroy(this->vehicleBody);
dGeomDestroy(this->vehicleGeom);
dRigidBodyArrayDestroy(this->bodyArray);
}
void ComponentMovement::resetMembers() {
dead = false;
velocity = vec3(0,0,0);
facingAngle = 0.0f;
maxForce = 5000;
lastAction = Stand;
lastReportedDeathBehavior = Corpse;
if (amotor) dJointDestroy(amotor);
if (lmotor) dJointDestroy(lmotor);
}
void PhysicsBallJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
}
_JointID = dJointCreateBall(getWorld()->getWorldID(), 0);
}
Inherited::changed(whichField, origin, details);
if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask))
{
dJointSetBallAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z());
}
}
void Spikey::unstick() {
if (hinge_) {
dJointDestroy(hinge_);
hinge_ = 0;
}
state_ = UNSTUCK;
}
void Spikey::stick(Body* b) {
if (hinge_) dJointDestroy(hinge_);
hinge_ = dJointCreateFixed(LEVEL->world, 0);
dJointAttach(hinge_, body_.body_id(), b ? b->body_id() : 0);
dJointSetFixed(hinge_);
state_ = STUCK;
}
/**
* This method is called if the joint should be detached. It stores
* the actual Joint-axis, which can change their orientation at
* Hinge2Joints, and destroys the helper body and joint.
**/
void Hinge2Joint::detachJoint() {
if (active && joint)
{
TransformationData entityTrans;
dVector3 vec;
dJointGetHinge2Axis1(joint, vec);
axis1[0] = vec[0];
axis1[1] = vec[1];
axis1[2] = vec[2];
dJointGetHinge2Axis2(joint, vec);
axis2[0] = vec[0];
axis2[1] = vec[1];
axis2[2] = vec[2];
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
gmtl::Quatf entityRot;
gmtl::Vec3f scaleVec;
gmtl::AxisAnglef axAng;
// get the inverse scale values of the mainEntity
/* scaleVec[0] = 1.0f/mainEntity->getXScale();
scaleVec[1] = 1.0f/mainEntity->getYScale();
scaleVec[2] = 1.0f/mainEntity->getZScale();*/
scaleVec[0] = 1.0f/entityTrans.scale[0];
scaleVec[1] = 1.0f/entityTrans.scale[1];
scaleVec[2] = 1.0f/entityTrans.scale[2];
// get the inverse Rotation of the mainEntity
// axAng[0] = -mainEntity->getRotAngle();
// axAng[1] = mainEntity->getXRot();
// axAng[2] = mainEntity->getYRot();
// axAng[3] = mainEntity->getZRot();
// gmtl::set(entityRot, axAng);
entityRot = entityTrans.orientation;
gmtl::invert(entityRot);
axis1 *= entityRot;
axis2 *= entityRot;
axis1[0] *= scaleVec[0];
axis1[1] *= scaleVec[1];
axis1[2] *= scaleVec[2];
axis2[0] *= scaleVec[0];
axis2[1] *= scaleVec[1];
axis2[2] *= scaleVec[2];
gmtl::normalize(axis1);
gmtl::normalize(axis2);
} // if
} // if
if (usedHelperJoint)
{
dJointDestroy(helperJoint);
dBodyDestroy(helperBody);
usedHelperJoint = false;
} // if
} // detachJoint
Joint::~Joint()
{
if (joint)
dJointDestroy(joint);
for (int i=0; i < (int)constraints.size(); i++)
delete constraints[i];
} // ~Joint
Joint::~Joint()
{
if (m_joint) {
dJointDestroy(m_joint);
}
if (m_jfb) {
delete m_jfb; m_jfb = NULL;
}
}
void BoxObstacle::remove() {
dGeomDestroy(boxGeom_);
if (box_ != 0) {
for(int i=0; i< dBodyGetNumJoints(box_); i++) {
dJointDestroy(dBodyGetJoint(box_, i));
}
dBodyDestroy(box_);
}
}
void ODE_Dynamixel::clear() {
ODE_Joint::clearJoint();
Dynamixel::clear();
if(mHingeJoint != 0) {
dJointDestroy(mHingeJoint);
mHingeJoint = 0;
}
mJoint = 0;
}
void IoODEJoint_free(IoODEJoint *self)
{
if(JOINTID && JOINTGROUP)
{
IoODEJointGroup_removeJoint(JOINTGROUP, self);
dJointDestroy(JOINTID);
}
free(IoObject_dataPointer(self));
}
void set_phys_join_type(dBodyID body1, dBodyID body2, int t)
{
dJointID joint = find_shared_joint(body1, body2);
if (joint)
dJointDestroy(joint);
if ((joint = create_phys_joint(t)))
dJointAttach(joint, body1, body2);
}
PhysicsActor::~PhysicsActor(){
if (joint>0)
dJointDestroy(joint);
if (geom>0)
dGeomDestroy(geom);
if (body>0)
dBodyDestroy(body);
}
/**
* This method is called if the Joint should be detached.
* It first calls the virtual method detachJoint, so that
* each type of Joint can store relevant information (e.g axis-angles)
* before the joint is deleted.
**/
void Joint::detach()
{
detachJoint();
if (joint)
{
dJointDestroy(joint);
joint = NULL;
} // if
alreadyAttached = false;
} // detach
void SParts::releaseObj()
{
m_graspObj.clear();
dJointDestroy(m_grasp_jointID);
m_grasp_jointID = NULL;
m_onGrasp = false;
m_gini[0] = 1.0;
m_gini[1] = 0.0;
m_gini[2] = 0.0;
m_gini[3] = 0.0;
}
/*
* Test if adhesions points should unattach due to (pulling) forces (vs AdhesionForce)
* Two different case : if stress pushing or stress pulling
* Update the number of points attached (and by effet de bord, the AdhesionForce)
* Return 1 if detached , else 0
*/
int StickyObj::testAdhesionEfficiency(){
//For each adhesionPoint:
// if (pull force + stress)*func(sheer) > Fadhesion --> then detach this point
dReal lengX = absdReal(2*collidingPointPos[0].getX()); // approximate the width of the conctact surfac with one point (should be done with all points ..)
dReal lengZ = absdReal(2*collidingPointPos[0].getZ()); // approximate the height of the conctact surfac with one point (should be done with all points ..)
dReal stepX = lengX/BOUND;
dReal stepZ = lengZ/BOUND;
dReal dispX = -lengX/2+stepX/2;
dReal dispZ = -lengZ/2+stepZ/2;
rho= adheringPoints/MAX_ADHESIONPOINTS;
dReal sigmaVonMisesThreshold = 200*ADHESION_FORCE_FACTOR*surfaceArea*rho; // = 1000 exp data from Webots
dReal thetaVonMisesThreshold = 50*ADHESION_FORCE_FACTOR*surfaceArea*rho; // guessed value
dReal mu = 0.5;
int adhePoints = MAX_ADHESIONPOINTS;
dReal max =0.0;
dReal maxTemp;
int i;
int j;
for(i=0; i< BOUND; i++){
for(j=0; j<BOUND;j++){
sheerAndStress[i][j] = computeSheerAndStress(dispX+stepX*i ,dispZ+stepZ*j);
maxTemp = 0.01*vonMisesFunction(sheerAndStress[i][j]);
if(sheerAndStress[i][j].getX() > 0){
if(maxTemp > sigmaVonMisesThreshold){
adhePoints--;
}
}else{
if(maxTemp > mu*-1*sheerAndStress[i][j].getX()*rho + thetaVonMisesThreshold){
adhePoints--;
}
}
if(maxTemp > max){
max = maxTemp;
}
}
}
adheringPoints = adhePoints;
if(adheringPoints == 0){
dWebotsConsolePrintf("Detached due to forces");
printInfos();
detach();
dJointDestroy(adhesionJoint);
return 1;
}
return 0;
}
void SkidSteeringVehicle::destroy() {
dBodyDestroy(this->vehicleBody);
dGeomDestroy(this->vehicleGeom);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
dBodyDestroy(this->wheelBody[fr][lr]);
dGeomDestroy(this->wheelGeom[fr][lr]);
dJointDestroy(this->wheelJoint[fr][lr]);
}
}
dRigidBodyArrayDestroy(this->bodyArray);
}
void Machine::destroy(void)
{
int i;
dBodyDestroy(body[0]);
dBodyDestroy(body[1]);
dGeomDestroy(geom[0]);
dJointDestroy(joint);
for(i=0; i<2; i++)
dGeomDestroy(geom[i+2]);
for(i=0; i<3; i++)
{
dBodyDestroy(wheel[i]);
dGeomDestroy(sphere[i]);
}
}
void ODE_Link::destroy()
{
if(jointType!=FREE_JOINT)
dJointDestroy(odeJointId);
for(int i=0; i<geomIds.size(); i++)
dGeomDestroy(geomIds.at(i));
if(triMeshDataId)
dGeomTriMeshDataDestroy(triMeshDataId);
dBodyDestroy(bodyId);
ODE_Link* link = static_cast<ODE_Link*>(child);
while(link){
ODE_Link* linkToDelete = (ODE_Link*)link;
link = static_cast<ODE_Link*>(link->sibling);
linkToDelete->destroy();
}
}
int main( void ) {
dWorldID world = dWorldCreate();
dJointID joint = dJointCreateBall( world, 0 );
dVector3 pos;
printf( "Create world and joint. Setting joint anchor to [4,11.18,-1.2]...\n" );
dJointSetBallAnchor( joint, 4.0, 11.18, -1.2 );
printf( "Done. Fetching set values...\n" );
dJointGetBallAnchor( joint, pos );
printf( "Anchor is at: [%0.5f, %0.5f, %0.5f]", pos[0], pos[1], pos[2] );
dJointDestroy( joint );
dWorldDestroy( world );
return 0;
}
void WorldPhysics::AI() {
if (tmp_wait>0) {tmp_wait--;return;}
if (bulldozer_state==8) speed=0;
const dReal* BPosition=bulldozer->getPosition();
// const dReal* BRotation=bulldozer->getRotation();
dReal minDistance=150; //~100 * sqrt(2)
Item* tmp;
int currentItemN=-1;
if (items.size()==0) return;
for (int i=0;i<items.size();i++) {
const dReal* ItemPosition=items[i]->getPosition();
if (ItemPosition[2]<0) items[i]->state=2;
if (items[i]->state==2) continue;
if (items[i]->state==1) {currentItemN=i;break;}
if (items[i]->state==0) {
dReal tmpmin=sqrt((ItemPosition[0]-BPosition[0])*
(ItemPosition[0]-BPosition[0]) +
(ItemPosition[1]-BPosition[1])*
(ItemPosition[1]-BPosition[1]));
if (tmpmin<minDistance) {
minDistance=tmpmin;
currentItemN=i;
}
}
}
if (currentItemN==-1) {
//bulldozer_state=0;
generateItems();
return;
}
/*
for (int i=0;i<items.size();i++) {
const dReal* tmp_pos=items[i]->getPosition();
if (tmp_pos[2]<0) items[i]->state=2;
if (items[i]->state==1) {
currentItemN=i;
break;
}
if (items[i]->state==2) currentItemN++;
}
if (currentItemN>=items.size()) return;
*/
tmp=items[currentItemN];
const dReal* ItemPosition=tmp->getPosition();
switch (tmp->state) {
case 0: { // Выбрали нужный кубик
tmp->state=1;
bulldozer_state=1; // Поворот до кубика
} break;
case 1: { // Надо к нему подъехать
switch (bulldozer_state) {
case 1:{
speed=0;
if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) bulldozer_state=2;
} break;
case 2: {
if (RotateTo(ItemPosition[0]-BPosition[0],ItemPosition[1]-BPosition[1])) {
speed=3;
if (((ItemPosition[0]-BPosition[0])*
(ItemPosition[0]-BPosition[0]) +
(ItemPosition[1]-BPosition[1])*
(ItemPosition[1]-BPosition[1]))<50) {
speed=0;
bulldozer_state=3;
}
} else {
speed=0;
bulldozer_state=1;
}
} break;
case 3: {
cheat_joint=dJointCreateSlider(world,0);
dJointAttach(cheat_joint,bulldozer->body,tmp->body);
dJointSetSliderAxis(cheat_joint,0,0,1);
//dJointSetSliderParam (cheat_joint, dParamCFM, 0.5);
bulldozer_state=4;
}break;
case 4: {
if (RotateTo(100-BPosition[0],0-BPosition[1])) bulldozer_state=5;
}break;
case 5: {
if (RotateTo(100-BPosition[0],0-BPosition[1])) {
speed=3;
if (sqrt((100-BPosition[0])*
(100-BPosition[0]) +
(0-BPosition[1])*
(0-BPosition[1]))<(sqrt(2)*25+LENGTH/2+RADIUS)) {
speed=0;
bulldozer_state=6;
}
} else {
speed=0;
bulldozer_state=4;
}
} break;
case 6: {
dJointDestroy(cheat_joint);
dBodyAddForce(tmp->body,100-BPosition[0],0-BPosition[1], 5);
bulldozer_state=7;
} break;
case 7: {
speed=-10;
tmp_wait=70;
bulldozer_state=8;
} break;
case 8: {
speed=0;
tmp->state=2;
} break;
}
} break;
}
}
void JointSlider::Create(World &world)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateSlider(world.Id(), 0);
}
ODEConstraint::~ODEConstraint()
{
if (m_odeJoint)
dJointDestroy(m_odeJoint);
}
void StickyObj::cleanUp(){
dJointDestroy(linkJoint);
dJointDestroy(adhesionJoint);
}
void PhysicsSliderJoint::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_JointID)
{
dJointDestroy(_JointID);
_JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
}
else
{
_JointID = dJointCreateSlider(getWorld()->getWorldID(), 0);
if(!(whichField & HiStopFieldMask))
{
setHiStop(dJointGetSliderParam(_JointID,dParamHiStop));
}
if(!(whichField & LoStopFieldMask))
{
setLoStop(dJointGetSliderParam(_JointID,dParamLoStop));
}
if(!(whichField & BounceFieldMask))
{
setBounce(dJointGetSliderParam(_JointID,dParamBounce));
}
if(!(whichField & CFMFieldMask))
{
setCFM(dJointGetSliderParam(_JointID,dParamCFM));
}
if(!(whichField & StopCFMFieldMask))
{
setStopCFM(dJointGetSliderParam(_JointID,dParamStopCFM));
}
if(!(whichField & StopERPFieldMask))
{
setStopERP(dJointGetSliderParam(_JointID,dParamStopERP));
}
}
}
Inherited::changed(whichField, origin, details);
if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
}
if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamHiStop, getHiStop());
}
if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamLoStop, getLoStop());
}
if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamBounce, getBounce());
}
if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamCFM, getCFM());
}
if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamStopERP, getStopERP());
}
if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
{
dJointSetSliderParam(_JointID, dParamStopCFM, getStopCFM());
}
}
//Overloaded Create
void JointSlider::Create(World &world, JointGroup &jointGroup)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateSlider(world.Id(), jointGroup.Id());
}
//Overloaded Create
void JointFixed::Create(World &world, JointGroup &jointGroup)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateFixed(world.Id(), jointGroup.Id());
}
sODEJoint::~sODEJoint()
{
dJointDestroy( oJoint );
}
void JointFixed::Create(World &world)
{
if(this->_id) dJointDestroy(this->_id);
_id = dJointCreateFixed(world.Id(), 0);
}
