/***************************************************************
* Function: setInitPosition()
***************************************************************/
void CAVEGeodeSnapSolidshape::setInitPosition(const osg::Vec3 &initPos, bool snap)
{
if (snap)
{
/* round intersected position to integer multiples of 'mSnappingUnitDist' */
Vec3 initPosRounded;
float snapUnitX, snapUnitY, snapUnitZ;
snapUnitX = snapUnitY = snapUnitZ = mSnappingUnitDist;
if (initPos.x() < 0)
snapUnitX = -mSnappingUnitDist;
if (initPos.y() < 0)
snapUnitY = -mSnappingUnitDist;
if (initPos.z() < 0)
snapUnitZ = -mSnappingUnitDist;
int xSeg = (int)(abs((int)((initPos.x() + 0.5 * snapUnitX) / mSnappingUnitDist)));
int ySeg = (int)(abs((int)((initPos.y() + 0.5 * snapUnitY) / mSnappingUnitDist)));
int zSeg = (int)(abs((int)((initPos.z() + 0.5 * snapUnitZ) / mSnappingUnitDist)));
initPosRounded.x() = xSeg * snapUnitX;
initPosRounded.y() = ySeg * snapUnitY;
initPosRounded.z() = zSeg * snapUnitZ;
mInitPosition = initPosRounded;
}
else
{
mInitPosition = initPos;
}
}
void daeWriter::writeUpdateTransformElements(const osg::Vec3 &pos, const osg::Quat &q, const osg::Vec3 &s)
{
// Make a scale place element
domScale *scale = daeSafeCast< domScale >( currentNode->add( COLLADA_ELEMENT_SCALE ) );
scale->setSid("scale");
scale->getValue().append3( s.x(), s.y(), s.z() );
// Make a three rotate place elements for the euler angles
// TODO decompose quaternion into three euler angles
double angle;
osg::Vec3 axis;
q.getRotate( angle, axis );
domRotate *rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateZ");
rot->getValue().append4( 0, 0, 1, osg::RadiansToDegrees(angle) );
rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateY");
rot->getValue().append4( 0, 1, 0, osg::RadiansToDegrees(angle) );
rot = daeSafeCast< domRotate >( currentNode->add( COLLADA_ELEMENT_ROTATE ) );
rot->setSid("rotateX");
rot->getValue().append4( 1, 0, 0, osg::RadiansToDegrees(angle) );
// Make a translate place element
domTranslate *trans = daeSafeCast< domTranslate >( currentNode->add( COLLADA_ELEMENT_TRANSLATE ) );
trans->setSid("translate");
trans->getValue().append3( pos.x(), pos.y(), pos.z() );
}
osg::MatrixTransform* createOffOriginOSGBox( osg::Vec3 size )
{
const osg::Vec3 dim( size * 2 );
osg::Geode * geode = new osg::Geode;
osg::Geometry * geom = new osg::Geometry;
osg::Vec3Array * v = new osg::Vec3Array;
v->resize( 8 );
( *v )[ 0 ] = osg::Vec3( 0, 0, 0 );
( *v )[ 1 ] = osg::Vec3( 0, dim.y(), 0 );
( *v )[ 2 ] = osg::Vec3( dim.x(), dim.y(), 0 );
( *v )[ 3 ] = osg::Vec3( dim.x(), 0, 0 );
( *v )[ 4 ] = osg::Vec3( 0, 0, dim.z() );
( *v )[ 5 ] = osg::Vec3( dim.x(), 0, dim.z() );
( *v )[ 6 ] = osg::Vec3( dim.x(), dim.y(), dim.z() );
( *v )[ 7 ] = osg::Vec3( 0, dim.y(), dim.z() );
geom->setVertexArray( v );
osg::Vec3Array * n = new osg::Vec3Array;
n->resize( 6 );
( *n )[ 0 ] = osg::Vec3( 0, 0, -1 );
( *n )[ 1 ] = osg::Vec3( 0, 0, 1 );
( *n )[ 2 ] = osg::Vec3( -1, 0, 0 );
( *n )[ 3 ] = osg::Vec3( 1, 0, 0 );
( *n )[ 4 ] = osg::Vec3( 0, -1, 0 );
( *n )[ 5 ] = osg::Vec3( 0, 1, 0 );
geom->setNormalArray( n );
geom->setNormalBinding( osg::Geometry::BIND_PER_PRIMITIVE );
osg::Vec4Array * c = new osg::Vec4Array;
c->resize( 8 );
( *c )[ 0 ] = osg::Vec4( 1, 1, 1, 1 );
( *c )[ 1 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 2 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 3 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 4 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 5 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 6 ] = osg::Vec4( .6, 0, 0, 1 );
( *c )[ 7 ] = osg::Vec4( .6, 0, 0, 1 );
geom->setColorArray( c );
geom->setColorBinding( osg::Geometry::BIND_PER_VERTEX );
GLushort indices[] =
{
0, 1, 2, 3,
4, 5, 6, 7,
0, 4, 7, 1,
3, 2, 6, 5,
0, 3, 5, 4,
1, 7, 6, 2
};
geom->addPrimitiveSet( new osg::DrawElementsUShort( GL_QUADS, 24, indices ) );
geode->addDrawable( geom );
osg::MatrixTransform * mt = new osg::MatrixTransform();
mt->addChild( geode );
return( mt );
}
void ComputeBoundingBoxByRotation(osg::BoundingBox & InOut,osg::Vec3 BBCenter,float BBSize,osg::Matrixd Rotation)
{
osg::BoundingBox bb;
bb.set(BBCenter.x()-BBSize,BBCenter.y()-BBSize,BBCenter.z()-BBSize,BBCenter.x()+BBSize,BBCenter.y()+BBSize,BBCenter.z()+BBSize);
osg::BoundingBox Tbb;
for(unsigned int i=0;i<8;i++)
{
Tbb.expandBy(Rotation.preMult(bb.corner(i)));
}
InOut.set(Tbb.xMin(),Tbb.yMin(),Tbb.zMin(),Tbb.xMax(),Tbb.yMax(),Tbb.zMax());
}
void PanoDrawableLOD::setZoom(osg::Vec3 dir, float k)
{
for(std::map<int,sph_model*>::iterator it = _pdi->modelMap.begin(); it!= _pdi->modelMap.end(); it++)
{
it->second->set_zoom(dir.x(),dir.y(),dir.z(),k);
}
for(std::map<int,sph_model*>::iterator it = _pdi->transitionModelMap.begin(); it!= _pdi->transitionModelMap.end(); it++)
{
it->second->set_zoom(dir.x(),dir.y(),dir.z(),k);
}
}
TouchSensor::TouchSensor(dSpaceID odeSpace, dBodyID sensorBody, float mass,
osg::Vec3 pos, float x, float y, float z, std::string label) :
collideSpace_(odeSpace), label_(label) {
// create own space to avoid physical collisions with other objects
sensorSpace_ = dHashSpaceCreate(0);
// create Sensor geom in this different space
dMass massOde;
dMassSetBoxTotal(&massOde, mass, x, y, z);
dBodySetMass(sensorBody, &massOde);
sensorGeometry_ = dCreateBox(sensorSpace_, x, y, z);
dBodySetPosition(sensorBody, pos.x(), pos.y(), pos.z());
dGeomSetPosition(sensorGeometry_, pos.x(), pos.y(), pos.z());
dGeomSetBody(sensorGeometry_, sensorBody);
}
bool HeightFieldLayer::getValue(unsigned int i, unsigned int j, osg::Vec3& value) const
{
value.x() = _heightField->getHeight(i,j);
value.y() = _defaultValue.y();
value.z() = _defaultValue.z();
return true;
}
void setMinMax(const osg::Vec3& pos)
{
_maxY = std::max(_maxY, (double) pos.y());
_minY = std::min(_minY, (double) pos.y());
_maxX = std::max(_maxX, (double) pos.x());
_minX = std::min(_minX, (double) pos.x());
}
void addEdgePlane(const osg::Vec3& corner, const osg::Vec3& edge)
{
osg::Vec3 normal( edge.y(), -edge.x(), 0.0f);
normal.normalize();
add(osg::Plane(normal, corner));
}
osg::Vec3 ConfigManager::getVec3(std::string attributeX, std::string attributeY,
std::string attributeZ, std::string path, osg::Vec3 def, bool * found)
{
bool hasEntry = false;
bool isFound;
osg::Vec3 result;
result.x() = getFloat(attributeX,path,def.x(),&isFound);
if(isFound)
{
hasEntry = true;
}
result.y() = getFloat(attributeY,path,def.y(),&isFound);
if(isFound)
{
hasEntry = true;
}
result.z() = getFloat(attributeZ,path,def.z(),&isFound);
if(isFound)
{
hasEntry = true;
}
if(found)
{
*found = hasEntry;
}
return result;
}
osg::Vec2 calcCoordReprojTrans(const osg::Vec3 &vert,const osg::Matrix &trans,const osg::Matrix &viewProj,const osg::Vec2 &size,const osg::Vec4 &ratio){
osg::Vec4 v(vert.x(),vert.y(),vert.z(),1.0);
v=v*trans;
v=v*viewProj;
v.x() /= v.w();
v.y() /= v.w();
v.z() /= v.w();
v.w() /= v.w();
//std::cout << "Pre shift " << v << std::endl;
v.x() /= size.x();;
v.y() /= size.y();
v.x() -= (ratio.x()/size.x());
v.y() -= (ratio.y()/size.y());
//std::cout << "Post shift " << v << std::endl;
// std::cout << "PP shift " << v << std::endl;
osg::Vec2 tc(v.x(),v.y());
tc.x() *= ratio.z();
tc.y() *=ratio.w();
//tc.x()*=ratio.x();
//tc.y()*=ratio.y();
tc.x()/=(ratio.z());
tc.y()/=(ratio.w());
return tc;
}
osg::Quat makeQuat(const osg::Vec3& radians, ERotationOrder fbxRotOrder)
{
fbxDouble3 degrees(
osg::RadiansToDegrees(radians.x()),
osg::RadiansToDegrees(radians.y()),
osg::RadiansToDegrees(radians.z()));
return makeQuat(degrees, fbxRotOrder);
}
void addRandomClouds(osg::Group* model, std::vector<Ref<CloudModel> >& clouds, osg::Vec3 dimensions, int count) {
std::vector<Ref<CloudModel> >::size_type cloudIndex = 0;
for(int i=0;i<count;++i, ++cloudIndex) {
// Wrap around when there is no more clouds.
if(cloudIndex >= clouds.size())
cloudIndex = 0;
float x = GenerateRandomNumber(0 - dimensions.x(), dimensions.x());
float y = GenerateRandomNumber(0 - dimensions.y(), dimensions.y());
float z = GenerateRandomNumber(0 - dimensions.z(), dimensions.z());
osg::ref_ptr<osg::MatrixTransform> transformation = new osg::MatrixTransform();
transformation->setMatrix(osg::Matrix::translate(x, y, z));
transformation->addChild(clouds.at(cloudIndex)->getNode());
model->addChild(transformation.get());
}
}
osg::Vec3 Particle_Viewer::fixPosition(osg::Vec3 position)
{
osg::Vec3 fixPosition;
fixPosition.x()=position.x();
fixPosition.y()=position.z();
fixPosition.z()=position.y();
return fixPosition;
}
void VELOModel::addTrack(osg::Vec3& v1, osg::Vec3& v2){
float d = sqrt(pow(v2.x()-v1.x(), 2) + pow(v2.y()-v1.y(), 2) + pow(v2.z()-v1.z(), 2));
osg::ref_ptr<osg::MatrixTransform> t2 = new osg::MatrixTransform;
t2->setMatrix( osg::Matrix::scale(1.0f, 1.0f, d) );
t2->addChild( _trackGeode.get() );
osg::ref_ptr<osg::MatrixTransform> t3 = new osg::MatrixTransform;
t3->setMatrix( osg::Matrix::rotate(osg::Vec3f(0.0f, 0.0f, 1.0f), v2 - v1) );
t3->addChild( t2.get() );
osg::ref_ptr<osg::MatrixTransform> t1 = new osg::MatrixTransform;
t1->setMatrix( osg::Matrix::translate(osg::Vec3f(((v2.x() - v1.x()) / 2.0f) + v1.x(), ((v2.y() - v1.y()) / 2.0f) + v1.y(), ((v2.z() - v1.z()) / 2.0f) + v1.z())) );
t1->addChild( t3.get() );
tracks->addChild( t1.get() );
}
void ForestTechniqueManager::createTreeList(osg::Node* terrain,const osg::Vec3& origin, const osg::Vec3& size,unsigned int numTreesToCreate,TreeList& trees)
{
float max_TreeHeight = sqrtf(size.length2()/(float)numTreesToCreate);
float max_TreeWidth = max_TreeHeight*0.5f;
float min_TreeHeight = max_TreeHeight*0.3f;
float min_TreeWidth = min_TreeHeight*0.5f;
trees.reserve(trees.size()+numTreesToCreate);
for(unsigned int i=0;i<numTreesToCreate;++i)
{
Tree* tree = new Tree;
tree->_position.set(random(origin.x(),origin.x()+size.x()),random(origin.y(),origin.y()+size.y()),origin.z());
tree->_color.set(random(128,255),random(128,255),random(128,255),255);
tree->_width = random(min_TreeWidth,max_TreeWidth);
tree->_height = random(min_TreeHeight,max_TreeHeight);
tree->_type = 0;
if (terrain)
{
osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector =
new osgUtil::LineSegmentIntersector(tree->_position,tree->_position+osg::Vec3(0.0f,0.0f,size.z()));
osgUtil::IntersectionVisitor iv(intersector.get());
terrain->accept(iv);
if (intersector->containsIntersections())
{
osgUtil::LineSegmentIntersector::Intersections& intersections = intersector->getIntersections();
for(osgUtil::LineSegmentIntersector::Intersections::iterator itr = intersections.begin();
itr != intersections.end();
++itr)
{
const osgUtil::LineSegmentIntersector::Intersection& intersection = *itr;
tree->_position = intersection.getWorldIntersectPoint();
}
}
}
trees.push_back(tree);
}
}
float HfT_osg_Plugin01_ParametricSurface::abstand(osg::Vec3 startPickPos, int i)
{
float x = (*mp_Points_Geom)[i].x() - startPickPos.x();
float y = (*mp_Points_Geom)[i].y() - startPickPos.y();
float z = (*mp_Points_Geom)[i].z() - startPickPos.z();
float abst = (float)sqrt(x * x + y * y + z * z);
return abst;
}
bool Uniform::getElement( unsigned int index, osg::Vec3& v3 ) const
{
if( index>=getNumElements() || !isCompatibleType(FLOAT_VEC3) ) return false;
unsigned int j = index * getTypeNumComponents(getType());
v3.x() = (*_floatArray)[j];
v3.y() = (*_floatArray)[j+1];
v3.z() = (*_floatArray)[j+2];
return true;
}
//
// Set the lighting material attributes for this osgmaterial
//
void HogBoxMaterial::SetMaterial(osg::Vec3 ambi, osg::Vec3 diffuse, osg::Vec3 spec, double exp)
{
//store the material values
_ambientColour = ambi;
osg::Vec4 ambi4 = osg::Vec4(ambi.x(), ambi.y(), ambi.z(), _alpha);
_diffuseColor = diffuse;
osg::Vec4 diffuse4 = osg::Vec4(diffuse.x(), diffuse.y(), diffuse.z(), _alpha);
_specularColour = spec;
osg::Vec4 spec4 = osg::Vec4(spec.x(), spec.y(), spec.z(), _alpha);
_shininess = exp;
//set the ambient colour
_material->setAmbient(_lightFace, ambi4);
//set the diffuse colour
_material->setDiffuse(_lightFace, diffuse4);
//set the specular colour
_material->setSpecular(_lightFace, spec4);
//set the shininess
_material->setShininess(_lightFace, _shininess);
_material->setColorMode(osg::Material::OFF);
//apply the material to the stateset
#ifdef OSG_GL_FIXED_FUNCTION_AVAILABLE
_stateset->setAttributeAndModes(_material.get(), osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON);
//normalise
_stateset->setMode( GL_NORMALIZE, osg::StateAttribute::ON);
#endif
this->SetTwoSidedLightingEnabled(_backFaceLit);
//set hogbox material uniforms
_ambientColourUni->set(_ambientColour);
_diffuseColourUni->set(_diffuseColor);
_specularColourUni->set(_specularColour);
_specularExpUni->set((float)_shininess);
_alphaUni->set((float)_alpha);
_twoSidedUni->set((int)_backFaceLit);
}
TexturedCuboid::TexturedCuboid(const osg::Vec3 &from, const osg::Vec3 &size) :
Cuboid(from, size)
{
_textureDrawable = new osg::Geometry();
osg::Vec3Array *pyramidVertices = new osg::Vec3Array();
{
pyramidVertices->push_back( from + osg::Vec3(0, 0, size.z()));
pyramidVertices->push_back( from + osg::Vec3(size.x(), 0, size.z()));
pyramidVertices->push_back( from + osg::Vec3(size.x(), size.y(), size.z()));
pyramidVertices->push_back( from + osg::Vec3(0, size.y(), size.z()));
}
_textureDrawable->setVertexArray( pyramidVertices );
osg::DrawElementsUInt *face = new osg::DrawElementsUInt(osg::PrimitiveSet::QUADS, 0);
face->push_back(0);
face->push_back(1);
face->push_back(2);
face->push_back(3);
_textureDrawable->addPrimitiveSet(face);
osg::Vec2Array* texcoords = new osg::Vec2Array(4);
{
(*texcoords)[0].set(size.x(), 0.0f);
(*texcoords)[1].set(0.0f, 0.0f);
(*texcoords)[3].set(size.x(), size.y() / 3.0f);
(*texcoords)[2].set(0.0f, size.y() / 3.0f);
_textureDrawable->setTexCoordArray(0, texcoords);
}
_texture = new osg::Texture2D;
_texture->setDataVariance(osg::Object::DYNAMIC);
_texture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT);
_texture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT);
osg::StateSet* stateSet = _textureDrawable->getOrCreateStateSet();
stateSet->setTextureAttributeAndModes(0, _texture, osg::StateAttribute::ON);
_textureDrawable->setStateSet(stateSet);
addDrawable(_textureDrawable);
}
void DataOutputStream::writeVec3(const osg::Vec3& v){
// std::cout << "write " << _ostream->tellp() << std::endl;
writeFloat(v.x());
writeFloat(v.y());
writeFloat(v.z());
if (_verboseOutput) std::cout<<"read/writeVec3() ["<<v<<"]"<<std::endl;
}
void VELOModel::addHit(osg::Vec3& coords){
osg::ref_ptr<osg::MatrixTransform> t1 = new osg::MatrixTransform;
t1->setMatrix( osg::Matrix::translate(
coords.x() - HIT_RADIUS,
coords.y(),
coords.z()) );
t1->addChild( _hitGeode.get() );
hits->addChild( t1.get() );
}
inline osg::Vec3 ReaderWriterOBJ::transformNormal(const osg::Vec3& vec, const bool rotate) const
{
if(rotate==true)
{
return osg::Vec3(vec.x(),-vec.z(),vec.y());
}
else
{
return vec;
}
}
bool ContourLayer::getValue(unsigned int i, unsigned int j, osg::Vec3& value) const
{
if (!_tf) return false;
const osg::Vec4& v = _tf->getPixelValue(i);
value.x() = v.x();
value.y() = v.y();
value.z() = v.z();
return true;
}
double CVehicle::calculateAngle(osg::Vec3 vecA,osg::Vec3 vecB)
{
double angle,angleA,angleB;
angle = acos(vecA * vecB / (vecA.length() * vecB.length()));
if( angle < 0.0001)
{
angle = 0.0;
return angle;
}
//求得vecA vecB分别与x轴正向的夹角
angleA = acos( vecA.x() / vecA.length() );
if( vecA.y() >= 0 )
angleA = angleA;
if( vecA.y() < 0 )
angleA = 2.0 * osg::PI - angleA;
angleB = acos( vecB.x() / vecB.length() );
if( vecB.y() >= 0 )
angleB = angleB;
if( vecB.y() < 0 )
angleB = 2.0 * osg::PI - angleB;
//得到两向量间的夹角，通过angleA angleB的关系来判断旋转的方向
if( angleA > angleB && (angleA - angleB) <= osg::PI )
angle = -1.0 * acos(vecA * vecB / (vecA.length() * vecB.length()));
if( angleA > angleB && (angleA - angleB) > osg::PI)
angle = acos(vecA * vecB / (vecA.length() * vecB.length())) ;
if( angleA < angleB && (angleB - angleA) <= osg::PI )
angle = acos(vecA * vecB / (vecA.length() * vecB.length()));
if( angleA < angleB && (angleB - angleA) > osg::PI)
angle = -1.0 * acos(vecA * vecB / (vecA.length() * vecB.length())) ;
return angle;
}
virtual bool handle( const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter& aa, osg::Object* pObject, osg::NodeVisitor* pNodeVisitor )
{
osgViewer::Viewer* pViewer = dynamic_cast<osgViewer::Viewer*>(&aa);
if ( !pViewer )
{
return false;
}
if ( ea.getEventType()==osgGA::GUIEventAdapter::KEYDOWN )
{
if ( ea.getKey()==osgGA::GUIEventAdapter::KEY_Up )
{
pos.z() += 1.0f;
updateTransform();
}
if ( ea.getKey()==osgGA::GUIEventAdapter::KEY_Down )
{
pos.z() -= 1.0f;
updateTransform();
}
if ( ea.getKey()==osgGA::GUIEventAdapter::KEY_Left )
{
pos.x() -= 1.0f;
updateTransform();
}
if ( ea.getKey()==osgGA::GUIEventAdapter::KEY_Right )
{
pos.x() += 1.0f;
updateTransform();
}
g_pSI->reset();
g_pSI->setPosition( pos );
testCol();
}
return false;
}
void PanoManipulator::JumpTo(osg::Vec3 NewSpatialPos)
{
PartialVertexDEM JumpLoc;
JumpLoc.Lat = NewSpatialPos.y();
JumpLoc.Lon = NewSpatialPos.x();
JumpLoc.Elev = NewSpatialPos.z();
MasterScene.DegToCart(JumpLoc);
_eye[0] = JumpLoc.XYZ[0];
_eye[1] = JumpLoc.XYZ[1];
_eye[2] = JumpLoc.XYZ[2];
} // PanoManipulator::JumpTo
void VertexCollectionVisitor::addVertex(osg::Vec3 vertex)
{
if (_geocentric)
{
double lat, lon, height;
_ellipsoidModel->convertXYZToLatLongHeight(vertex.x(), vertex.y(), vertex.z(), lat, lon, height);
_vertices->push_back(osg::Vec3d(osg::RadiansToDegrees(lon), osg::RadiansToDegrees(lat), height));
}
else
{
_vertices->push_back(vertex);
}
}
void CameraFlight::navigate(osg::Matrix destMat, osg::Vec3 destVec)
{
osg::Matrix objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
switch(_flightMode)
{
case INSTANT:{
cout<<"USING INSTANT"<<endl;
SceneManager::instance()->setObjectMatrix(destMat);
break;
}
case SATELLITE:
cout<<"USING SATELLITE"<<endl;
t = 0.0;
total = 0.0;
objMat.decompose(trans2, rot2, scale2, so2);
a = (maxHeight - trans2[1])/25.0;
map->getProfile()->getSRS()->getEllipsoid()->convertLatLongHeightToXYZ(
destVec.x(),destVec.y(),destVec.z(),toVec.x(),toVec.y(),toVec.z());
fromVec = origPlanetPoint;
fromVec.normalize();
toVec.normalize();
origAngle = acos((fromVec * toVec)/((fromVec.length() * toVec.length())));
origAngle = RadiansToDegrees(origAngle);
angle = origAngle;
if(origAngle <= 10) {
maxHeight = 6.5e+9;
}
else {
maxHeight = 2.0e+10;
}
flagRot = true;
break;
case AIRPLANE:
cout<<"USING AIRPLANE"<<endl;
break;
default:
cout<<"PICK THE ALGORYTHM!!!!"<<endl;
break;
}
}
BoxObstacle::BoxObstacle(dWorldID odeWorld, dSpaceID odeSpace,
const osg::Vec3& pos, const osg::Vec3& size, float density,
const osg::Vec3& rotationAxis, float rotationAngle) :
size_(size) {
if (density >= RobogenUtils::OSG_EPSILON_2){
// if not fixed, create body
box_ = dBodyCreate(odeWorld);
dMass massOde;
dMassSetBox(&massOde, density, size.x(), size.y(), size.z());
dBodySetMass(box_, &massOde);
} else{
// otherwise make body 0
box_ = 0;
}
boxGeom_ = dCreateBox(odeSpace, size.x(), size.y(), size.z());
dGeomSetBody(boxGeom_, box_);
dGeomSetPosition(boxGeom_, pos.x(), pos.y(), pos.z());
// for some reason body/geom position do not get tied together as they
// should, so we set the body position as well, and use it when getting
// the position on non-stationary bodies
if (box_ != 0)
dBodySetPosition(box_, pos.x(), pos.y(), pos.z());
if (rotationAngle >= RobogenUtils::OSG_EPSILON_2){
osg::Quat rotation;
rotation.makeRotate(rotationAngle,rotationAxis);
dQuaternion quatOde;
quatOde[0] = rotation.w();
quatOde[1] = rotation.x();
quatOde[2] = rotation.y();
quatOde[3] = rotation.z();
dGeomSetQuaternion(boxGeom_, quatOde);
}
}