double Frustum::getAngleBetween(const osg::Vec3 &a, const osg::Vec3 &b) {
float dot = a * b;
float mag = a.length() * b.length();
double angle = acos(dot/mag);
if (osg::isNaN(angle))
return 0;
else
return angle;
}
float ConeSector::operator() (const osg::Vec3& eyeLocal) const
{
float dotproduct = eyeLocal*_axis;
float length = eyeLocal.length();
if (dotproduct>_cosAngle*length) return 1.0f; // fully in sector
if (dotproduct<_cosAngleFade*length) return 0.0f; // out of sector
return (dotproduct-_cosAngleFade*length)/((_cosAngle-_cosAngleFade)*length);
}
osg::Vec3 ShadowTechnique::computeOrthogonalVector(const osg::Vec3& direction) const
{
float length = direction.length();
osg::Vec3 orthogonalVector = direction ^ osg::Vec3(0.0f, 1.0f, 0.0f);
if (orthogonalVector.normalize()<length*0.5f)
{
orthogonalVector = direction ^ osg::Vec3(0.0f, 0.0f, 1.0f);
orthogonalVector.normalize();
}
return orthogonalVector;
}
void set(const osg::Vec3d& start, osg::Vec3d& end, float ratio=FLT_MAX)
{
_hit=false;
_index = 0;
_ratio = ratio;
_s = start;
_d = end - start;
_length = _d.length();
_d /= _length;
}
void GliderManipulator::computePosition(const osg::Vec3& eye,const osg::Vec3& lv,const osg::Vec3& up)
{
osg::Vec3 f(lv);
f.normalize();
osg::Vec3 s(f^up);
s.normalize();
osg::Vec3 u(s^f);
u.normalize();
osg::Matrixd rotation_matrix(s[0], u[0], -f[0], 0.0f,
s[1], u[1], -f[1], 0.0f,
s[2], u[2], -f[2], 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
_eye = eye;
_distance = lv.length();
_rotation = rotation_matrix.getRotate().inverse();
}
osg::ref_ptr<osg::Geode> createRefGeometry( osg::Vec3 p, double len )
{
osg::ref_ptr<osg::Geode> geode = new osg::Geode;
if ( _drawingFlag==1 )
{
osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
// Joint
vertices->push_back( osg::Vec3(-len, 0.0, 0.0) );
vertices->push_back( osg::Vec3( len, 0.0, 0.0) );
vertices->push_back( osg::Vec3( 0.0,-len, 0.0) );
vertices->push_back( osg::Vec3( 0.0, len, 0.0) );
vertices->push_back( osg::Vec3( 0.0, 0.0,-len) );
vertices->push_back( osg::Vec3( 0.0, 0.0, len) );
// Bone
vertices->push_back( osg::Vec3( 0.0, 0.0, 0.0) );
vertices->push_back( p );
geometry->addPrimitiveSet( new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 8) );
geometry->setVertexArray( vertices.get() );
geode->addDrawable( geometry.get() );
}
else if ( _drawingFlag==2 )
{
osg::Quat quat;
osg::ref_ptr<osg::Box> box = new osg::Box( p*0.5, p.length(), len, len );
quat.makeRotate( osg::Vec3(1.0,0.0,0.0), p );
box->setRotation( quat );
geode->addDrawable( new osg::ShapeDrawable(box.get()) );
}
return geode;
}
void ConstraintsNode::applyConstrainedTranslation(osg::Vec3 v)
{
osg::Vec3 localPos = this->getTranslation();
osg::ref_ptr<ReferencedNode> hitNode;
ReferencedNode *testNode;
// with really big velocities (or small enclosures), and if the surface
// doesn't damp the velocity, it's possible to get see an infinite recursion
// occur. So, we keep a recursionCounter, and stop prevent this occurrence:
if (++recursionCounter > 10) return;
/*
std::cout << std::endl << "checking for collisions" << std::endl;
std::cout << "start = " << localPos.x()<<","<<localPos.y()<<","<<localPos.z() << std::endl;
std::cout << "v = " << v.x()<<","<<v.y()<<","<<v.z() << std::endl;
*/
osg::ref_ptr<ReferencedNode> targetNode = dynamic_cast<ReferencedNode*>(_target->s_thing);
if (targetNode.valid())
{
// get current position (including offset from previous bounces)
osg::Matrix thisMatrix = osg::computeLocalToWorld(this->currentNodePath);
osg::Vec3 worldPos = thisMatrix.getTrans();
// set up intersector:
osg::ref_ptr<osgUtil::LineSegmentIntersector> intersector = new osgUtil::LineSegmentIntersector(worldPos, worldPos + v);
osgUtil::IntersectionVisitor iv(intersector.get());
// apply intersector:
targetNode->accept(iv);
if (intersector->containsIntersections())
{
osgUtil::LineSegmentIntersector::Intersections intersections;
osgUtil::LineSegmentIntersector::Intersections::iterator itr;
intersections = intersector->getIntersections();
for (itr = intersections.begin(); itr != intersections.end(); ++itr)
{
//std::cout << "testing intersection with " << (*itr).nodePath[0]->getName() << std::endl;
// first check if the hit is with our target (should be first in
// the nodepath):
testNode = dynamic_cast<ReferencedNode*>((*itr).nodePath[0]);
if (testNode!=targetNode) continue;
// The intersection has a nodepath that we have to walk down to
// see which exact node has been hit. It's possible that there
// are other SPIN nodes in the targetNode's subgraph that are
// the real source of intersection:
hitNode = 0;
testNode = 0;
for (unsigned int i=0; i<(*itr).nodePath.size(); i++)
{
testNode = dynamic_cast<ReferencedNode*>((*itr).nodePath[i]);
if (testNode) hitNode = testNode;
}
if (hitNode.valid())
{
//std::cout << id->s_name << " collision!!! with " << (*itr).drawable->getName() << "[" << (*itr).primitiveIndex << "] @ " << std::endl;
//std::cout << "localHitPoint:\t" << localHitPoint.x()<<","<<localHitPoint.y()<<","<<localHitPoint.z() << std::endl;
//std::cout << "localHitNormal:\t" << localHitNormal.x()<<","<<localHitNormal.y()<<","<<localHitNormal.z() << std::endl;
// For BOUNCE mode, we need to check if we've intersected
// with the same primitive again. This may occur since we
// do recursive translations after repositioning the node at
// the bounce point (and there may be numerical imprecision)
// If so, we skip this intersection:
if ((_mode==BOUNCE) &&
(lastDrawable.get()==(*itr).drawable.get()) &&
(lastPrimitiveIndex==(int)(*itr).primitiveIndex))
{
//std::cout << "... skipping" << std::endl;
continue;
}
else
{
lastDrawable=(*itr).drawable;
lastPrimitiveIndex=(*itr).primitiveIndex;
}
osg::Vec3 localHitPoint = (*itr).getWorldIntersectPoint();
osg::Vec3 localHitNormal = (*itr).getWorldIntersectNormal();
localHitNormal.normalize();
// current direction vector:
osg::Vec3 dirVec = v;
dirVec.normalize();
// Find the rotation between the direction vector and the
// surface normal at the hit point:
osg::Quat rot;
rot.makeRotate(dirVec, localHitNormal);
//std::cout << "rot = " << rot.x()<<","<<rot.y()<<","<<rot.z()<<","<<rot.w() << " ... angle=" << acos(rot.w())*2 << ", indegrees=" << osg::RadiansToDegrees(acos(rot.w()))*2 << std::endl;
// the surface normal may be in the opposite direction
// from us. If so, we need to flip it:
if (acos(rot.w()) * 2 > osg::PI_2)
{
localHitNormal *= -1;
rot.makeRotate(dirVec, localHitNormal);
//std::cout << "flipped = " << rot.x()<<","<<rot.y()<<","<<rot.z()<<","<<rot.w() << " ... angle=" << acos(rot.w())*2 << ", indegrees=" << osg::RadiansToDegrees(acos(rot.w()))*2 << std::endl;
}
osg::Vec3 rotEulers = QuatToEuler(rot);
//std::cout << "newHitNormal:\t" << localHitNormal.x()<<","<<localHitNormal.y()<<","<<localHitNormal.z() << std::endl;
if ((_mode==COLLIDE)||(_mode==COLLIDE_THRU)||(_mode==STICK))
{
// Let the collisionPoint be just a bit before the real
// hitpoint (to avoid numerical imprecision placing the
// node behind the plane):
osg::Vec3 collisionPoint = localHitPoint - (localHitNormal * 0.01);
// place the node at the collision point
setTranslation(collisionPoint.x(), collisionPoint.y(), collisionPoint.z());
BROADCAST(this, "ssfff", "collide", hitNode->id->s_name, osg::RadiansToDegrees(rotEulers.x()), osg::RadiansToDegrees(rotEulers.y()), osg::RadiansToDegrees(rotEulers.z()));
if (_mode==COLLIDE)
{
// SLIDE along the hit plane with the left over energy:
// ie, project the remaining vector onto the surface we
// just intersected with.
//
// using:
// cos(theta) = distToSurface / remainderVector length
//
double cosTheta = (dirVec * -localHitNormal) ; // dot product
osg::Vec3 remainderVector = (localPos + v) - localHitPoint;
double distToSurface = cosTheta * remainderVector.length();
osg::Vec3 slideVector = remainderVector + (localHitNormal * distToSurface);
// pseudo-recursively apply remainder of bounce:
applyConstrainedTranslation( slideVector );
}
else if (_mode==COLLIDE_THRU)
{
// allow the node to pass thru (ie, just apply the
// translation):
//setTranslation(v.x(), v.y(), v.z());
osg::Vec3 newPos = localPos + v;
setTranslation(newPos.x(), newPos.y(), newPos.z());
}
return;
}
else if (_mode==BOUNCE)
{
// bounce returns a translation mirrored about the hit
// normal to the surface
// the new direction vector is a rotated version
// of the original, about the localHitNormal:
//osg::Vec3 newDir = (rot * 2.0) * -dirVec;
osg::Vec3 newDir = (rot * (rot * -dirVec));
newDir.normalize();
//std::cout << "newDir = " << newDir.x()<<","<<newDir.y()<<","<<newDir.z() << std::endl;
// amount of distance still to travel after bounce:
double dist = v.length() - (localHitPoint-localPos).length();
//std::cout << "dist = " << dist << std::endl;
// in node is translating (ie, changing position
// independently from it's orientatino), then we need to
// flip the velocity vector.
if (_velocityMode == GroupNode::TRANSLATE)
{
osg::Vec3 newVel = newDir * _velocity.length();
setVelocity(newVel.x(), newVel.y(), newVel.z());
}
// if the node is moving along it's orientation vector,
// we need to update the orientation of so that it
// points in 'bounced' direction
else if (_velocityMode == GroupNode::MOVE)
{
osg::Quat newQuat;
newQuat.makeRotate(osg::Vec3(0,1,0), newDir);
osg::Vec3 newRot = Vec3inDegrees(QuatToEuler(newQuat));
//std::cout << "newrot = " << newRot.x()<<","<<newRot.y()<<","<<newRot.z() << std::endl;
setOrientation(newRot.x(), newRot.y(), newRot.z());
}
// the new position will be just a hair before hitpoint
// (because numerical imprecision may place the hitpoint
// slightly beyond the surface, and when we bounce the
// node, we don't want to intersect with the same
// surface again)
double HAIR = 0.0000001;
setTranslation(localHitPoint.x()-dirVec.x()*HAIR, localHitPoint.y()-dirVec.y()*HAIR, localHitPoint.z()-dirVec.z()*HAIR);
//setTranslation(localHitPoint.x(), localHitPoint.y(), localHitPoint.z());
//std::cout << "rotEulers = " << osg::RadiansToDegrees(rotEulers.x())<<","<<osg::RadiansToDegrees(rotEulers.y())<<","<<osg::RadiansToDegrees(rotEulers.z()) << std::endl;
BROADCAST(this, "ssfff", "bounce", hitNode->id->s_name, osg::RadiansToDegrees(rotEulers.x()), osg::RadiansToDegrees(rotEulers.y()), osg::RadiansToDegrees(rotEulers.z()));
// pseudo-recursively apply remainder of bounce:
applyConstrainedTranslation(newDir*dist);
return;
}
}
}
} //else std::cout << "no intersections" << std::endl;
}
// no intersections, so just do regular translation:
osg::Vec3 newPos = localPos + v;
setTranslation(newPos.x(), newPos.y(), newPos.z());
}
float cvr::planePointDistanceRef(const osg::Vec3 & planePoint,
const osg::Vec3 & planeNormal, const osg::Vec3 & point)
{
return ((point - planePoint) * planeNormal) / planeNormal.length();
}
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;
}
void setAxis(const osg::Vec3 &a)
{
axis_ = a / a.length();
}