OSG::NodeTransitPtr createLabeledTorus(OSG::Vec3f trans, int idx)
{
OSG::NodeTransitPtr node = OSG::Node::create();
OSG::TransformRefPtr xform = OSG::Transform::create();
OSG::Matrix mat;
// --- setup transform ------------------
mat.setIdentity();
mat.setTranslate(trans);
xform->setMatrix(mat);
node->setCore(xform);
// --- setup label ----------------------
OSG::NodeRefPtr labelNode = OSG::Node::create();
OSG::LabelRefPtr label =
(idx) ? createTextLabel(idx) : createIconLabel();
updateLabelParams(label, idx);
labelNode->setCore(label);
// --- add torus ------------------------
labelNode->addChild(OSG::makeTorus(.5, 2, 16, 16));
node->addChild(labelNode);
return node;
}
OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result,
OSG::Pnt3f from,
OSG::Pnt3f at,
OSG::Vec3f up )
{
Vec3f view;
Vec3f right;
Vec3f newup;
Vec3f tmp;
view = from - at;
view.normalize();
right = up.cross(view);
if(right.dot(right) < TypeTraits<Real>::getDefaultEps())
{
return true;
}
right.normalize();
newup = view.cross(right);
result.setIdentity ();
result.setTranslate(from[0], from[1], from[2]);
Matrix tmpm;
tmpm.setValue(right, newup, view);
result.mult(tmpm);
return false;
}
void readScaleElement(FbxPropertyT<FbxDouble3>& prop,
osgAnimation::UpdateMatrixTransform* pUpdate,
osg::Matrix& staticTransform,
FbxScene& fbxScene)
{
FbxDouble3 fbxPropValue = prop.Get();
osg::Vec3d val(
fbxPropValue[0],
fbxPropValue[1],
fbxPropValue[2]);
if (isAnimated(prop, fbxScene))
{
if (!staticTransform.isIdentity())
{
pUpdate->getStackedTransforms().push_back(new osgAnimation::StackedMatrixElement(staticTransform));
staticTransform.makeIdentity();
}
pUpdate->getStackedTransforms().push_back(new osgAnimation::StackedScaleElement("scale", val));
}
else
{
staticTransform.preMultScale(val);
}
}
void
SoundUpdateCB::operator()( osg::Node* node, osg::NodeVisitor* nv )
{
const osg::FrameStamp* fs( nv->getFrameStamp() );
if( !m_sound_state.valid() || ( fs == NULL ) )
{
// Early exit.
osg::notify(osg::DEBUG_INFO) << "SoundUpdateCB::operator()() No SoundState attached, or invalid FrameStamp." << std::endl;
traverse( node, nv );
return;
}
const double t( fs->getReferenceTime() );
const double time( t - m_last_time );
if(time >= m_sound_manager->getUpdateFrequency())
{
const osg::Matrix m( osg::computeLocalToWorld( nv->getNodePath() ) );
const osg::Vec3 pos = m.getTrans();
m_sound_state->setPosition(pos);
//Calculate velocity
osg::Vec3 velocity(0,0,0);
if (m_first_run)
{
m_first_run = false;
m_last_time = t;
m_last_pos = pos;
}
else
{
velocity = pos - m_last_pos;
m_last_pos = pos;
m_last_time = t;
velocity /= time;
}
if(m_sound_manager->getClampVelocity())
{
float max_vel = m_sound_manager->getMaxVelocity();
float len = velocity.length();
if ( len > max_vel)
{
velocity.normalize();
velocity *= max_vel;
}
}
m_sound_state->setVelocity(velocity);
//Get new direction
osg::Vec3 dir = osg::Vec3( 0., 1., 0. ) * m;
dir.normalize();
m_sound_state->setDirection(dir);
// Only do occlusion calculations if the sound is playing
if (m_sound_state->getPlay() && m_occlude_callback.valid())
m_occlude_callback->apply(m_sound_manager->getListenerMatrix(), pos, m_sound_state.get());
} // if time to update
traverse( node, nv );
}
void coTouchIntersection::intersect(const osg::Matrix &handMat, bool mouseHit)
{
cerr << "coTouchIntersection::intersect info: called" << endl;
Vec3 q0, q1, q2, q3, q4, q5;
// 3 line segments for interscetion test hand-object
// of course this test can fail
q0.set(0.0f, -0.5f * handSize, 0.0f);
q1.set(0.0f, 0.5f * handSize, 0.0f);
q2.set(-0.5f * handSize, 0.0f, 0.0f);
q3.set(0.5f * handSize, 0.0f, 0.0f);
q4.set(0.0f, 0.0f, -0.5f * handSize);
q5.set(0.0f, 0.0f, 0.5f * handSize);
// xform the intersection line segment
q0 = handMat.preMult(q0);
q1 = handMat.preMult(q1);
q2 = handMat.preMult(q2);
q3 = handMat.preMult(q3);
q4 = handMat.preMult(q4);
q5 = handMat.preMult(q5);
ref_ptr<LineSegment> ray1 = new LineSegment();
ref_ptr<LineSegment> ray2 = new LineSegment();
ref_ptr<LineSegment> ray3 = new LineSegment();
ray1->set(q0, q1);
ray2->set(q2, q3);
ray3->set(q4, q5);
IntersectVisitor visitor;
visitor.addLineSegment(ray1.get());
visitor.addLineSegment(ray2.get());
visitor.addLineSegment(ray3.get());
cover->getScene()->traverse(visitor);
ref_ptr<LineSegment> hitRay = 0;
if (visitor.getNumHits(ray1.get()))
hitRay = ray1;
else if (visitor.getNumHits(ray2.get()))
hitRay = ray2;
else if (visitor.getNumHits(ray3.get()))
hitRay = ray3;
if (visitor.getNumHits(hitRay.get()))
{
Hit hitInformation = visitor.getHitList(hitRay.get()).front();
cover->intersectionHitPointWorld = hitInformation.getWorldIntersectPoint();
cover->intersectionHitPointLocal = hitInformation.getLocalIntersectPoint();
cover->intersectionMatrix = hitInformation._matrix;
cover->intersectedNode = hitInformation._geode;
// walk up to the root and call all coActions
OSGVruiHit hit(hitInformation, mouseHit);
OSGVruiNode node(cover->intersectedNode.get());
callActions(&node, &hit);
}
}
// redraw the window
void display( void )
{
// create the matrix
OSG::Matrix m;
OSG::Real32 t = glutGet(GLUT_ELAPSED_TIME);
// set the transforms' matrices
m.setTransform(OSG::Vec3f(0,
0,
OSG::osgSin(t / 1000.f) * 1.5),
OSG::Quaternion(OSG::Vec3f (1, 0, 0),
t / 500.f));
cyltrans->setMatrix(m);
m.setTransform(OSG::Vec3f(OSG::osgSin(t / 2000.f),
0,
0),
OSG::Quaternion(OSG::Vec3f (0, 0, 1),
t / 2000.f));
tortrans->setMatrix(m);
mgr->redraw();
}
void readScaleElement(KFbxTypedProperty<fbxDouble3>& prop,
osgAnimation::UpdateMatrixTransform* pUpdate,
osg::Matrix& staticTransform)
{
fbxDouble3 fbxPropValue = prop.Get();
osg::Vec3d val(
fbxPropValue[0],
fbxPropValue[1],
fbxPropValue[2]);
if (prop.GetKFCurve(KFCURVENODE_S_X) ||
prop.GetKFCurve(KFCURVENODE_S_Y) ||
prop.GetKFCurve(KFCURVENODE_S_Z))
{
if (!staticTransform.isIdentity())
{
pUpdate->getStackedTransforms().push_back(new osgAnimation::StackedMatrixElement(staticTransform));
staticTransform.makeIdentity();
}
pUpdate->getStackedTransforms().push_back(new osgAnimation::StackedScaleElement("scale", val));
}
else
{
staticTransform.preMultScale(val);
}
}
btCollisionShape* VRPhysics::getConvexShape() {
btConvexHullShape* shape = new btConvexHullShape();
OSG::Matrix m;
OSG::Matrix M = vr_obj->getWorldMatrix();
M.invert();
vector<OSG::VRObject*> geos = vr_obj->getObjectListByType("Geometry");
for (auto g : geos) {
OSG::VRGeometry* geo = (OSG::VRGeometry*)g;
if (geo == 0) continue;
if (geo->getMesh() == 0) continue;
OSG::GeoVectorPropertyRecPtr pos = geo->getMesh()->getPositions();
if (pos == 0) continue;
if (geo != vr_obj) {
m = geo->getWorldMatrix();
m.multLeft(M);
}
for (unsigned int i = 0; i<pos->size(); i++) {
OSG::Pnt3f p;
pos->getValue(p,i);
if (geo != vr_obj) m.mult(p,p);
for (int i=0; i<3; i++) p[i] *= scale[i];
shape->addPoint(btVector3(p[0], p[1], p[2]));
}
}
shape->setMargin(collisionMargin);
//cout << "\nConstruct Convex shape for " << vr_obj->getName() << endl;
return shape;
}
void vehiclePoseCallback(const nav_msgs::Odometry& odom) {
if (!firstpass) {
initialT.set(odom.pose.pose.position.x,odom.pose.pose.position.y,odom.pose.pose.position.z);
initialQ.set(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
wMv_initial.setTrans(initialT);
wMv_initial.setRotate(initialQ);
firstpass=true;
}
}
OSG::Matrix VRPhysics::fromBTTransform(const btTransform t, OSG::Vec3f scale) {
OSG::Matrix m = fromBTTransform(t);
// apply scale
OSG::Matrix s;
s.setScale(scale);
m.mult(s);
return m;
}
void Transform::setTranslation(OSG::Real32 x, OSG::Real32 y, OSG::Real32 z)
{
OSG::Matrix m;
m.setTransform(OSG::Vec3f(x,y,z));
// OSG::beginEditCP(_transform);
_transform->setMatrix(m);
// OSG::endEditCP(_transform);
}
void Particles::setModelTransformation(TransformationData trans)
{
OSG::Matrix m;
gmtl::Matrix44f mat;
transformationDataToMatrix(trans, mat);
m.setValue(mat.getData());
beginEditCP(particleModelTrans, Transform::MatrixFieldMask);
particleModelTrans->setMatrix(m);
endEditCP(particleModelTrans, Transform::MatrixFieldMask);
} // Particles
void copyOsgMatrixToLib3dsMatrix(Lib3dsMatrix lib3ds_matrix, const osg::Matrix& osg_matrix)
{
for(int row=0; row<4; ++row)
{
lib3ds_matrix[row][0] = osg_matrix.ptr()[row*4+0];
lib3ds_matrix[row][1] = osg_matrix.ptr()[row*4+1];
lib3ds_matrix[row][2] = osg_matrix.ptr()[row*4+2];
lib3ds_matrix[row][3] = osg_matrix.ptr()[row*4+3];
}
}
OSG::NodeTransitPtr createScene(OSG::Window *win)
{
OSG::NodeRecPtr rootN = OSG::makeNodeFor(OSG::Group::create());
// Create ground:
OSG::NodeUnrecPtr groundN = OSG::makePlane(25,25,1,1);
OSG::Matrix m;
OSG::Quaternion q;
q.setValueAsAxisDeg(OSG::Vec3f(1,0,0), -90);
m.setRotate(q);
OSG::TransformUnrecPtr groundTransC = OSG::Transform::create();
groundTransC->setMatrix(m);
OSG::NodeUnrecPtr groundTransN = OSG::makeNodeFor(groundTransC);
groundTransN->addChild(groundN);
rootN->addChild(groundTransN);
// Set ground material:
OSG::SimpleMaterialUnrecPtr mat = OSG::SimpleMaterial::create();
mat->setDiffuse(OSG::Color3f(0.8,0.8,0.8));
dynamic_cast<OSG::Geometry*>(groundN->getCore())->setMaterial(mat);
// // Create figure:
// OSG::NodeUnrecPtr figure1N =
// OSG::SceneFileHandler::the()->read("../Models/Figure.obj");
// G.figure1TransC = OSG::Transform::create();
// OSG::NodeUnrecPtr trans1N = OSG::makeNodeFor(G.figure1TransC);
// trans1N->addChild(figure1N);
// rootN->addChild(trans1N);
OSG::NodeUnrecPtr figureModelA =
OSG::SceneFileHandler::the()->read("../assets/Figure.obj");
BoardGame::Figure* figureA = new BoardGame::Figure();
figureA->setModel(figureModelA);
rootN->addChild(figureA->getRoot());
OSG::NodeUnrecPtr figureModelB =
OSG::SceneFileHandler::the()->read("../assets/Figure.obj");
BoardGame::Figure* figureB = new BoardGame::Figure();
figureB->setModel(figureModelB);
rootN->addChild(figureB->getRoot());
G.selectedNode = figureModelA;
return(OSG::NodeTransitPtr(rootN));
}
// redraw the window
void display( void )
{
// create the matrix
OSG::Matrix m;
OSG::Real32 t = glutGet(GLUT_ELAPSED_TIME );
m.setTransform(OSG::Quaternion(OSG::Vec3f(0,1,0), t / 1000.f));
// set the transform's matrix
trans->setMatrix(m);
OSG::commitChanges();
mgr->redraw();
}
OSG_BEGIN_NAMESPACE
OSG_BASE_DLLMAPPING bool MatrixOrthogonal(OSG::Matrix &result,
OSG::Real32 rLeft,
OSG::Real32 rRight,
OSG::Real32 rBottom,
OSG::Real32 rTop,
OSG::Real32 rNear,
OSG::Real32 rFar)
{
result.setValueTransposed(
2.f / (rRight - rLeft),
0.f,
0.f,
0.f,
0.f,
2.f / (rTop - rBottom),
0.f,
0.f,
0.f,
0.f,
-2.f / (rFar - rNear),
0.f,
-(rRight + rLeft ) / (rRight - rLeft ),
-(rTop + rBottom) / (rTop - rBottom),
-(rFar + rNear ) / (rFar - rNear ),
1.);
return false;
}
void ScreenBase::computeDefaultViewProj(osg::Vec3d eyePos, osg::Matrix & view,
osg::Matrix & proj)
{
//translate screen to origin
osg::Matrix screenTrans;
screenTrans.makeTranslate(-_myInfo->xyz);
//rotate screen to xz
osg::Matrix screenRot;
screenRot.makeRotate(-_myInfo->h * M_PI / 180.0,osg::Vec3(0,0,1),
-_myInfo->p * M_PI / 180.0,osg::Vec3(1,0,0),
-_myInfo->r * M_PI / 180.0,osg::Vec3(0,1,0));
eyePos = eyePos * screenTrans * screenRot;
//make frustum
float top, bottom, left, right;
float screenDist = -eyePos.y();
top = _near * (_myInfo->height / 2.0 - eyePos.z()) / screenDist;
bottom = _near * (-_myInfo->height / 2.0 - eyePos.z()) / screenDist;
right = _near * (_myInfo->width / 2.0 - eyePos.x()) / screenDist;
left = _near * (-_myInfo->width / 2.0 - eyePos.x()) / screenDist;
proj.makeFrustum(left,right,bottom,top,_near,_far);
// move camera to origin
osg::Matrix cameraTrans;
cameraTrans.makeTranslate(-eyePos);
//make view
view = screenTrans * screenRot * cameraTrans
* osg::Matrix::lookAt(osg::Vec3(0,0,0),osg::Vec3(0,1,0),
osg::Vec3(0,0,1));
}
bool
AbsoluteModelTransform::computeWorldToLocalMatrix( osg::Matrix& matrix, osg::NodeVisitor* nv ) const
{
osg::Matrix inv( osg::Matrix::inverse( _matrix ) );
if( getReferenceFrame() == osg::Transform::ABSOLUTE_RF )
{
osg::Matrix invView;
if( !nv )
osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NULL NodeVisitor; can't get invView." << std::endl;
else if( nv->getVisitorType() != osg::NodeVisitor::CULL_VISITOR )
osg::notify( osg::NOTICE ) << "AbsoluteModelTransform: NodeVisitor is not CullVisitor; can't get invView." << std::endl;
else
{
osgUtil::CullVisitor* cv = dynamic_cast< osgUtil::CullVisitor* >( nv );
osg::Camera* cam = cv->getCurrentCamera();
cam->computeWorldToLocalMatrix( invView, cv );
}
matrix = ( invView * inv );
}
else
// RELATIVE_RF
matrix.postMult( inv );
return( true );
}
bool HUDSettings::getInverseModelViewMatrix(osg::Matrix& matrix, osg::NodeVisitor* nv) const
{
osg::Matrix modelView;
getModelViewMatrix(modelView,nv);
matrix.invert(modelView);
return true;
}
void CameraFlight::rotate(osg::Vec3 from, osg::Vec3 to)
{
objMat = SceneManager::instance()->getObjectTransform()->getMatrix();
objMat.decompose(trans1, rot1, scale1, so1);
rotAngle = (origAngle + (-(origAngle/25)*pow((t-5),2)))/195;
if(angle > 0.0) {
osg::Vec3 crsVec = from^to;
crsVec.normalize();
osg::Matrix rotM;
rotM.makeRotate(DegreesToRadians(-1*rotAngle),crsVec);
angle -= rotAngle;
_rotMat = rotM * objMat;
//SceneManager::instance()->setObjectMatrix(_rotMat);
}
else {
_rotMat = objMat;
}
zoomOut(trans1,_rotMat);
}
OSG::NodeTransitPtr createScene(void)
{
// create the scene:
OSG::TransformTransitPtr xform = OSG::Transform::create();
OSG::Matrix mat;
mat.setTranslate(OSG::Vec3f(2,0,0));
xform->setMatrix(mat);
OSG::NodeTransitPtr scene = OSG::Node::create();
scene->addChild(createLabeledScene());
scene->setCore(xform);
OSG::commitChanges();
return scene;
}
void PhysXInterface::setMatrix( int id, const osg::Matrix& matrix )
{
PxReal d[16];
for ( int i=0; i<16; ++i ) d[i] = *(matrix.ptr() + i);
PxRigidActor* actor = _actors[id];
if ( actor ) actor->setGlobalPose( PxTransform(PxMat44(d)) );
}
OSG::Vec3f getCurrentTranslation(OSG::Matrix& matrix)
{
OSG::Vec3f curTrans, scale;
OSG::Quaternion rot;
matrix.getTransform(curTrans, rot, scale, rot);
return(curTrans);
}
bool
GeoTransform::ComputeMatrixCallback::computeWorldToLocalMatrix(const GeoTransform* xform, osg::Matrix& m, osg::NodeVisitor* nv) const
{
if (xform->getReferenceFrame() == xform->RELATIVE_RF)
m.postMult(xform->getInverseMatrix());
else
m = xform->getInverseMatrix();
return true;
}
// create the motion matrix for a light source at time t
void makeMatrix(OSG::Real32 t, OSG::Matrix &result)
{
OSG::Matrix m;
result.setTransform(OSG::Quaternion(OSG::Vec3f(0,0,1), -OSG::Pi / 2));
m.setTransform(OSG::Vec3f(1, 0, 0));
result.multLeft(m);
m.setTransform(OSG::Quaternion(OSG::Vec3f(0,1,0), t / 1000.f));
result.multLeft(m);
m.setTransform(OSG::Vec3f(2, 0, 0));
result.multLeft(m);
m.setTransform(OSG::Quaternion(OSG::Vec3f(0,0,1), t / 3000.f));
result.multLeft(m);
}
OSG_BASE_DLLMAPPING bool MatrixPerspective(OSG::Matrix &result,
OSG::Real32 rFovy,
OSG::Real32 rAspect,
OSG::Real32 rNear,
OSG::Real32 rFar)
{
Real32 ct = osgTan(rFovy);
bool error = false;
if(rNear > rFar)
{
SWARNING << "MatrixPerspective: near " << rNear << " > far " << rFar
<< "!\n" << std::endl;
error = true;
}
if(rFovy <= TypeTraits<Real32>::getDefaultEps())
{
SWARNING << "MatrixPerspective: fovy " << rFovy << " very small!\n"
<< std::endl;
error = true;
}
if(osgAbs(rNear - rFar) < TypeTraits<Real32>::getDefaultEps())
{
SWARNING << "MatrixPerspective: near " << rNear << " ~= far " << rFar
<< "!\n" << std::endl;
error = true;
}
if(rAspect < TypeTraits<Real32>::getDefaultEps())
{
SWARNING << "MatrixPerspective: aspect ratio " << rAspect
<< " very small!\n" << std::endl;
error = true;
}
if(error)
{
result.setIdentity();
return true;
}
MatrixFrustum( result,
-rNear * ct * rAspect,
rNear * ct * rAspect,
-rNear * ct,
rNear * ct,
rNear,
rFar );
return false;
}
bool HUDSettings::getModelViewMatrix(osg::Matrix& matrix, osg::NodeVisitor* nv) const
{
matrix.makeLookAt(osg::Vec3d(0.0,0.0,0.0),osg::Vec3d(0.0,_slideDistance,0.0),osg::Vec3d(0.0,0.0,1.0));
if (nv)
{
if (nv->getTraversalMask()==_leftMask)
{
matrix.postMultTranslate(osg::Vec3(_eyeOffset,0.0,0.0));
}
else if (nv->getTraversalMask()==_rightMask)
{
matrix.postMultTranslate(osg::Vec3(-_eyeOffset,0.0,0.0));
}
}
return true;
}
virtual bool computeWorldToLocalMatrix( osg::Matrix& matrix,osg::NodeVisitor* nv ) const
{
osgUtil::CullVisitor* cv = dynamic_cast<osgUtil::CullVisitor*>( nv );
if ( cv ) {
osg::Vec3 eyePointLocal = cv->getEyeLocal();
matrix.postMult( osg::Matrix::translate( -eyePointLocal ) );
}
return true;
}
Property::Property( const std::string& _name, const osg::Matrix& _v )
{
name = normalize( _name );
std::stringstream ss;
const osg::Matrix::value_type* p = _v.ptr();
for( int i=0; i<15; i++ ) ss << *p++ << " ";
ss << *p;
value = ss.str();
valid = true;
}
//
// transform vector from world space to eye space
//
OSG::Vec3f transform_to_eye_space(const OSG::Vec3f& v, OSG::SimpleSceneManager* pSSM)
{
if (!pSSM || !pSSM->getWindow() || pSSM->getWindow()->getMFPort()->size() == 0)
return v;
OSG::Viewport* pPort = mgr->getWindow()->getPort(0);
OSG::Vec3f v_es;
OSG::Matrix view;
OSG::Int16 width = pPort->calcPixelWidth();
OSG::Int16 height = pPort->calcPixelHeight();
pPort->getCamera()->getViewing(view, width, height);
view.multFull( v, v_es);
return v_es;
}