int ClosedPolygon::insertIndexedPoint(const Vec3d &p)
{
int vIndex = -1;
kdres * findP = points.nearest3f(p.x(), p.y(), p.z());
if(findP)
{
double * pos = findP->riter->item->pos;
Vec3d closestPoint(pos[0], pos[1], pos[2]);
double dist = (closestPoint - p).norm();
if(dist < closedPolyEpsilon)
{
vIndex = findP->riter->item->index;
kd_res_free(findP);
return vIndex;
}
}
vIndex = lastVertexIndex;
allPoints[vIndex] = p;
points.insert3f(p.x(), p.y(), p.z(), lastVertexIndex++);
return vIndex;
}
//--------------------------------------------------------------------------------------------------
/// Test bounding box to see if it's outside the frustum or not
///
/// \return true if outside or exactly on the boundary. false if inside.
//--------------------------------------------------------------------------------------------------
bool Frustum::isOutside(const BoundingBox& bbox) const
{
CVF_ASSERT(m_planes.size() == 6);
const Vec3d& boxMin = bbox.min();
const Vec3d& boxMax = bbox.max();
Vec3d point;
Vec3d planeNormal;
std::map<int, Plane>::const_iterator it;
for (it = m_planes.begin(); it != m_planes.end(); it++)
{
planeNormal = it->second.normal();
point.x() = (planeNormal.x() <= 0.0) ? boxMin.x() : boxMax.x();
point.y() = (planeNormal.y() <= 0.0) ? boxMin.y() : boxMax.y();
point.z() = (planeNormal.z() <= 0.0) ? boxMin.z() : boxMax.z();
if (it->second.distanceSquared(point) < 0.0)
{
return true;
}
}
return false;
}
void SimpleDraw::DrawCircle( const Vec3d& center, double radius, const Vec4d& c, const Vec3d& n, float lineWidth )
{
Vec3d startV(0,0,0);
// Find orthogonal start vector
if ((abs(n.y()) >= 0.9f * abs(n.x())) &&
abs(n.z()) >= 0.9f * abs(n.x())) startV = Vec3d(0.0f, -n.z(), n.y());
else if ( abs(n.x()) >= 0.9f * abs(n.y()) &&
abs(n.z()) >= 0.9f * abs(n.y()) ) startV = Vec3d(-n.z(), 0.0f, n.x());
else startV = Vec3d(-n.y(), n.x(), 0.0f);
int segCount = 20;
double theta = 2.0 * M_PI / segCount;
glDisable(GL_LIGHTING);
glLineWidth(lineWidth);
glColor4dv(c);
glBegin(GL_LINE_LOOP);
for(int i = 0; i < segCount; i++){
glVertex3dv(center + startV * radius );
ROTATE_VEC(startV, theta, n);
}
glEnd();
glEnable(GL_LIGHTING);
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
void BoundingBox::add(const Vec3d& point)
{
if (point.x() < m_min.x()) m_min.x() = point.x();
if (point.y() < m_min.y()) m_min.y() = point.y();
if (point.z() < m_min.z()) m_min.z() = point.z();
if (point.x() > m_max.x()) m_max.x() = point.x();
if (point.y() > m_max.y()) m_max.y() = point.y();
if (point.z() > m_max.z()) m_max.z() = point.z();
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
TEST(VariantTest, TypeVec3d)
{
const Vec3d val(1.0, -2000.1234, 3000.6789);
Variant var(val);
ASSERT_EQ(Variant::VEC3D, var.type());
ASSERT_TRUE(var.isValid());
Vec3d v = var.getVec3d();
ASSERT_EQ(val.x(), v.x());
ASSERT_EQ(val.y(), v.y());
ASSERT_EQ(val.z(), v.z());
}
void virtuose::fillSpeed(VRPhysics* p, float* to, VRPhysics* origin) {
Vec3d vel = p->getLinearVelocity();
if (origin!=0) vel -= origin->getLinearVelocity();
to[0] = vel.z();
to[1] = vel.x();
to[2] = vel.y();
Vec3d ang = p->getAngularVelocity();
if (origin!=0) ang -= origin->getAngularVelocity();
to[3] = ang.z();
to[4] = ang.x();
to[5] = ang.y();
}
// Make a rotation Quat which will rotate vec1 to vec2
// Generally take adot product to get the angle between these
// and then use a cross product to get the rotation axis
// Watch out for the two special cases of when the vectors
// are co-incident or opposite in direction.
void Quat::makeRotate_original( const Vec3d& from, const Vec3d& to )
{
const value_type epsilon = 0.0000001;
value_type length1 = from.length();
value_type length2 = to.length();
// dot product vec1*vec2
value_type cosangle = from*to/(length1*length2);
if ( fabs(cosangle - 1) < epsilon )
{
OSG_INFO<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
// cosangle is close to 1, so the vectors are close to being coincident
// Need to generate an angle of zero with any vector we like
// We'll choose (1,0,0)
makeRotate( 0.0, 0.0, 0.0, 1.0 );
}
else
if ( fabs(cosangle + 1.0) < epsilon )
{
// vectors are close to being opposite, so will need to find a
// vector orthongonal to from to rotate about.
Vec3d tmp;
if (fabs(from.x())<fabs(from.y()))
if (fabs(from.x())<fabs(from.z())) tmp.set(1.0,0.0,0.0); // use x axis.
else tmp.set(0.0,0.0,1.0);
else if (fabs(from.y())<fabs(from.z())) tmp.set(0.0,1.0,0.0);
else tmp.set(0.0,0.0,1.0);
Vec3d fromd(from.x(),from.y(),from.z());
// find orthogonal axis.
Vec3d axis(fromd^tmp);
axis.normalize();
_v[0] = axis[0]; // sin of half angle of PI is 1.0.
_v[1] = axis[1]; // sin of half angle of PI is 1.0.
_v[2] = axis[2]; // sin of half angle of PI is 1.0.
_v[3] = 0; // cos of half angle of PI is zero.
}
else
{
// This is the usual situation - take a cross-product of vec1 and vec2
// and that is the axis around which to rotate.
Vec3d axis(from^to);
value_type angle = acos( cosangle );
makeRotate( angle, axis );
}
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
bool StructGridCutPlane::isCellIntersectedByPlane(const Plane& plane, const Vec3d& cellMinCoord, const Vec3d& cellMaxCoord)
{
// See http://zach.in.tu-clausthal.de/teaching/cg_literatur/lighthouse3d_view_frustum_culling/index.html
// Start by finding the "positive vertex" and the "negative vertex" relative to plane normal
Vec3d pVertex(cellMinCoord);
Vec3d nVertex(cellMaxCoord);
if (plane.A() >= 0)
{
pVertex.x() = cellMaxCoord.x();
nVertex.x() = cellMinCoord.x();
}
if (plane.B() >= 0)
{
pVertex.y() = cellMaxCoord.y();
nVertex.y() = cellMinCoord.y();
}
if (plane.C() >= 0)
{
pVertex.z() = cellMaxCoord.z();
nVertex.z() = cellMinCoord.z();
}
// Chek if both positive and negative vertex are on same side of plane
if (plane.distanceSquared(pVertex) < 0)
{
if (plane.distanceSquared(nVertex) < 0)
{
return false;
}
else
{
return true;
}
}
else
{
if (plane.distanceSquared(nVertex) >= 0)
{
return false;
}
else
{
return true;
}
}
}
//--------------------------------------------------------------------------------------------------
/// Check if the bounding box contains the specified point
///
/// Note that a point on the box's surface is classified as being contained
//--------------------------------------------------------------------------------------------------
bool BoundingBox::contains(const Vec3d& point) const
{
CVF_TIGHT_ASSERT(isValid());
if (point.x() >= m_min.x() && point.x() <= m_max.x() &&
point.y() >= m_min.y() && point.y() <= m_max.y() &&
point.z() >= m_min.z() && point.z() <= m_max.z())
{
return true;
}
else
{
return false;
}
}
void Octree::newNode( int depth, double x, double y, double z )
{
double extent = boundingBox.xExtent / 2.0;
Vec3d center;
center.x() = boundingBox.center.x() + (extent * x);
center.y() = boundingBox.center.y() + (extent * y);
center.z() = boundingBox.center.z() + (extent * z);
BoundingBox bb(center, extent, extent, extent);
// Add child
children.push_back(Octree());
Octree * child = &children.back();
child->boundingBox = bb;
child->trianglePerNode = this->trianglePerNode;
// Collect triangles inside child's bounding box
for(StdVector<BaseTriangle*>::iterator it = this->triangleData.begin(); it != this->triangleData.end(); it++)
{
BaseTriangle* face = *it;
if( bb.containsTriangle(face->vec(0), face->vec(1), face->vec(2)) )
{
child->triangleData.push_back(face);
}
}
child->build(depth + 1); // build it
}
pair<ofVec3f, ofVec3f> VDB::bbox() {
math::CoordBBox bbox = grid->evalActiveVoxelBoundingBox();
Coord minC = bbox.getStart();
Coord maxC = bbox.getEnd();
Vec3d minPt = grid->indexToWorld(minC);
Vec3d maxPt = grid->indexToWorld(maxC);
return make_pair(ofVec3f(minPt.x(), minPt.y(), minPt.z()), ofVec3f(maxPt.x(), maxPt.y(), maxPt.z()));
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
Vec3d StructGridCutPlane::planeLineIntersection(const Plane& plane, const Vec3d& p1, const Vec3d& p2, const double s1, const double s2, double* s)
{
// From http://local.wasp.uwa.edu.au/~pbourke/geometry/planeline/
//
// P1 (x1,y1,z1) and P2 (x2,y2,z2)
//
// P = P1 + u (P2 - P1)
//
// A*x1 + B*y1 + C*z1 + D
// u = ---------------------------------
// A*(x1-x2) + B*(y1-y2) + C*(z1-z2)
CVF_ASSERT(s);
const Vec3d v = p2 - p1;
double denominator = -(plane.A()*v.x() + plane.B()*v.y() + plane.C()*v.z());
if (denominator != 0)
{
double u = (plane.A()*p1.x() + plane.B()*p1.y() + plane.C()*p1.z() + plane.D())/denominator;
if (u > 0.0 && u < 1.0)
{
*s = s1 + u*(s2 - s1);
return (p1 + u*v);
}
else
{
if (u >= 1.0)
{
*s = s2;
return p2;
}
else
{
*s = s1;
return p1;
}
}
}
else
{
*s = s1;
return p1;
}
}
Vec3d scene::addVertex(Vec3d v)
{
v += _t;
v = preMultd(_r, v);
osg::Matrixd m = osg::Matrixd::translate(v.x(), v.y(), v.z());
m = m * _m;
Vec3d a = preMultd(m, Vec3d(0,0,0));
_b.expandBy(a);
return a;
}
void Camera::computeDepthRanges(const osg::Node *graph, std::vector<std::pair<double, double> > &ranges)
{
if (!graph || dynamic_cast<const osg::Camera*>(graph))
return;
bool bPopMatrix = false;
const PositionAttitudeTransform *ptrans = dynamic_cast<const PositionAttitudeTransform*>(graph);
if (ptrans)
{
const osg::Matrixd mat = Matrixd::translate(-ptrans->getPivotPoint())
* Matrixd::rotate(ptrans->getAttitude())
* Matrixd::scale(ptrans->getScale())
* Matrixd::translate(ptrans->getPosition());
pushMatrix(mat);
bPopMatrix = true;
}
const MatrixTransform *mtrans = dynamic_cast<const MatrixTransform*>(graph);
if (mtrans)
{
pushMatrix(mtrans->getMatrix());
bPopMatrix = true;
}
const Switch* sgroup = dynamic_cast<const Switch*>(graph);
if (sgroup)
{
for(unsigned int i = 0, nb = sgroup->getNumChildren() ; i < nb ; ++i)
if (sgroup->getValue(i))
computeDepthRanges(sgroup->getChild(i), ranges);
}
else
{
const Group* group = dynamic_cast<const Group*>(graph);
if (group)
{
for(unsigned int i = 0, nb = group->getNumChildren() ; i < nb ; ++i)
computeDepthRanges(group->getChild(i), ranges);
}
}
const Geode *geode = dynamic_cast<const Geode*>(graph);
if (geode)
{
const osg::BoundingSphered &bound = computeBoundsOf(graph);
const double r = bound.radius();
const Vec3d view_center = bound.center() * qMatrix.back();
const double zmin = -view_center.z() - r;
const double zmax = -view_center.z() + r;
ranges.push_back(std::make_pair(zmin, zmax));
}
if (bPopMatrix)
popMatrix();
}
//--------------------------------------------------------------------------------------------------
/// Compute the plane equation from the given point and normal
///
/// \param point Point on the plane
/// \param normal The plane's normal
//--------------------------------------------------------------------------------------------------
bool Plane::setFromPointAndNormal(const Vec3d& point, const Vec3d& normal)
{
if (normal.isZero()) return false;
m_A = normal.x();
m_B = normal.y();
m_C = normal.z();
m_D = -(normal*point);
return true;
}
osg::Vec3d CoordinateSystemNode::computeLocalUpVector(const Vec3d& position) const
{
if (_ellipsoidModel.valid())
{
return _ellipsoidModel->computeLocalUpVector(position.x(),position.y(),position.z());
}
else
{
return osg::Vec3d(0.0f,0.0f,1.0f);
}
}
void SimpleDraw::IdentifyPoint( const Vec3d & p, float r /*= 1.0*/, float g /*= 0.2f*/, float b /*= 0.2f*/, float pointSize /*= 10.0*/ )
{
glDisable(GL_LIGHTING);
// Colored dot
glColor3f(r, g, b);
glPointSize(pointSize);
glBegin(GL_POINTS);
glVertex3f(p.x(), p.y(), p.z());
glEnd();
// White Border
glPointSize(pointSize + 2);
glColor3f(1, 1, 1);
glBegin(GL_POINTS);
glVertex3f(p.x(), p.y(), p.z());
glEnd();
glEnable(GL_LIGHTING);
}
void SimpleDraw::DrawSphere( const Vec3d & center, float radius /*= 1.0f*/ )
{
glPushMatrix();
glTranslatef(center.x(), center.y(), center.z());
GLUquadricObj *quadObj = gluNewQuadric();
gluQuadricDrawStyle(quadObj, GLU_FILL);
gluQuadricNormals(quadObj, GLU_SMOOTH);
gluSphere(quadObj, radius, 16, 16);
gluDeleteQuadric(quadObj);
glPopMatrix();
}
CoordinateFrame CoordinateSystemNode::computeLocalCoordinateFrame(const Vec3d& position) const
{
if (_ellipsoidModel.valid())
{
Matrixd localToWorld;
double latitude, longitude, height;
_ellipsoidModel->convertXYZToLatLongHeight(position.x(),position.y(),position.z(),latitude, longitude, height);
_ellipsoidModel->computeLocalToWorldTransformFromLatLongHeight(latitude, longitude, 0.0f, localToWorld);
return localToWorld;
}
else
{
return Matrixd::translate(position.x(),position.y(),0.0f);
}
}
void SimpleDraw::DrawCube( const Vec3d & center, float length /*= 1.0f*/ )
{
static GLdouble n[6][3] ={
{-1.0, 0.0, 0.0},
{0.0, 1.0, 0.0},
{1.0, 0.0, 0.0},
{0.0, -1.0, 0.0},
{0.0, 0.0, 1.0},
{0.0, 0.0, -1.0}};
static GLint faces[6][4] ={
{0, 1, 2, 3},
{3, 2, 6, 7},
{7, 6, 5, 4},
{4, 5, 1, 0},
{5, 6, 2, 1},
{7, 4, 0, 3}};
GLdouble v[8][3];GLint i;
v[0][0] = v[1][0] = v[2][0] = v[3][0] = -length / 2;
v[4][0] = v[5][0] = v[6][0] = v[7][0] = length / 2;
v[0][1] = v[1][1] = v[4][1] = v[5][1] = -length / 2;
v[2][1] = v[3][1] = v[6][1] = v[7][1] = length / 2;
v[0][2] = v[3][2] = v[4][2] = v[7][2] = -length / 2;
v[1][2] = v[2][2] = v[5][2] = v[6][2] = length / 2;
glPushMatrix();
glTranslatef(center.x(), center.y(), center.z());
for (i = 0; i < 6; i++)
{
glBegin(GL_QUADS);
glNormal3dv(&n[i][0]);
glVertex3dv(&v[faces[i][0]][0]);
glVertex3dv(&v[faces[i][1]][0]);
glVertex3dv(&v[faces[i][2]][0]);
glVertex3dv(&v[faces[i][3]][0]);
glEnd();
}
glPopMatrix();
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}
void SimpleDraw::DrawCylinder( const Vec3d & center, const Vec3d & direction /*= Vec3d (0,0,1)*/,
float height, float radius /*= 1.0f*/, float radius2 /*= -1*/ )
{
glPushMatrix();
glTranslatef(center.x(), center.y(), center.z());
glMultMatrixd(qglviewer::Quaternion(qglviewer::Vec(0,0,1), qglviewer::Vec(direction)).matrix());
GLUquadricObj *quadObj = gluNewQuadric();
gluQuadricDrawStyle(quadObj, GLU_FILL);
gluQuadricNormals(quadObj, GLU_SMOOTH);
if(radius2 < 0) radius2 = radius;
gluCylinder(quadObj, radius, radius2, height, 16, 16);
gluDeleteQuadric(quadObj);
glPopMatrix();
}
std::vector<double> Cuboid::getCoordinate( Point v )
{
std::vector<double> coords(3);
// Map points to uniform box coordinates
QSurfaceMesh currBoxMesh = getGeometry();
QSurfaceMesh currUniformBoxMesh = getBoxGeometry(currBox, true);
v = MeanValueCooridnates::point(
MeanValueCooridnates::weights(v, &currBoxMesh), &currUniformBoxMesh);
Vec3d pos = getCoordinatesInUniformBox(currBox, v);
coords[0] = pos.x();
coords[1] = pos.y();
coords[2] = pos.z();
return coords;
}
void GraphicCamera::MouseEventHandler(int button, int state, int x, int y){
double realy, wx, wy, wz;
// if ALT key used
// int mode = glutGetModifiers();
if (state == GLUT_UP && CameraMode != kINACTIVE) {
current_elev += dt_elev;
current_azim += dt_azim;
//reset change in elevation and roll of the camera;
dt_elev = dt_azim = 0.0;
CameraMode = kINACTIVE;
} else if (state == GLUT_DOWN) {
MouseStartX = MousePrevX = x;
MouseStartY = MousePrevY = y;
switch(button) {
case GLUT_LEFT_BUTTON:
//rotating
CameraMode = kROTATE;
break;
case GLUT_MIDDLE_BUTTON:
// translating
CameraMode = kTRANSLATE;
glGetIntegerv(GL_VIEWPORT, ViewPort);
glGetDoublev(GL_MODELVIEW_MATRIX, MvMat.GetPtr());
glGetDoublev(GL_PROJECTION_MATRIX, ProjMat.GetPtr());
// height of window in pixels
realy = ViewPort[3] - y - 1;
gluProject(aim_.x(), aim_.y(), aim_.z(), MvMat.GetPtr(),
ProjMat.GetPtr(), ViewPort, &wx, &wy, &wz);
gluUnProject((GLdouble) x, (GLdouble) realy, wz, MvMat.GetPtr(),
ProjMat.GetPtr(), ViewPort, &PrevMousePos.x(),
&PrevMousePos.y(), &PrevMousePos.z());
break;
case GLUT_RIGHT_BUTTON:
CameraMode = kZOOM;
break;
}
}
}
Array* Array_readLocalData(Input& fr)
{
if (fr[0].matchWord("Use"))
{
if (fr[1].isString())
{
Object* obj = fr.getObjectForUniqueID(fr[1].getStr());
if (obj)
{
fr+=2;
return dynamic_cast<Array*>(obj);
}
}
osg::notify(osg::WARN)<<"Warning: invalid uniqueID found in file."<<std::endl;
return NULL;
}
std::string uniqueID;
if (fr[0].matchWord("UniqueID") && fr[1].isString())
{
uniqueID = fr[1].getStr();
fr += 2;
}
int entry = fr[0].getNoNestedBrackets();
const char* arrayName = fr[0].getStr();
unsigned int capacity = 0;
fr[1].getUInt(capacity);
++fr;
fr += 2;
Array* return_array = 0;
if (strcmp(arrayName,"ByteArray")==0)
{
ByteArray* array = new ByteArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
int int_value;
if (fr[0].getInt(int_value))
{
++fr;
array->push_back(int_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"ShortArray")==0)
{
ShortArray* array = new ShortArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
int int_value;
if (fr[0].getInt(int_value))
{
++fr;
array->push_back(int_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"IntArray")==0)
{
IntArray* array = new IntArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
int int_value;
if (fr[0].getInt(int_value))
{
++fr;
array->push_back(int_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"UByteArray")==0)
{
UByteArray* array = new UByteArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int uint_value;
if (fr[0].getUInt(uint_value))
{
++fr;
array->push_back(uint_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"UShortArray")==0)
{
UShortArray* array = new UShortArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int uint_value;
if (fr[0].getUInt(uint_value))
{
++fr;
array->push_back(uint_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"UIntArray")==0)
{
UIntArray* array = new UIntArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int uint_value;
if (fr[0].getUInt(uint_value))
{
++fr;
array->push_back(uint_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"UVec4bArray")==0 || strcmp(arrayName,"Vec4ubArray")==0)
{
Vec4ubArray* array = new Vec4ubArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g,b,a;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g) &&
fr[2].getUInt(b) &&
fr[3].getUInt(a))
{
fr+=4;
array->push_back(osg::Vec4ub(r,g,b,a));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"FloatArray")==0)
{
FloatArray* array = new FloatArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
float float_value;
if (fr[0].getFloat(float_value))
{
++fr;
array->push_back(float_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"DoubleArray")==0)
{
DoubleArray* array = new DoubleArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
double double_value;
if (fr[0].getFloat(double_value))
{
++fr;
array->push_back(double_value);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec2Array")==0)
{
Vec2Array* array = new Vec2Array;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec2 v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()))
{
fr += 2;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec2dArray")==0)
{
Vec2dArray* array = new Vec2dArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec2d v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()))
{
fr += 2;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec3Array")==0)
{
Vec3Array* array = new Vec3Array;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec3 v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()) && fr[2].getFloat(v.z()))
{
fr += 3;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec3dArray")==0)
{
Vec3dArray* array = new Vec3dArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec3d v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()) && fr[2].getFloat(v.z()))
{
fr += 3;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec4Array")==0)
{
Vec4Array* array = new Vec4Array;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec4 v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()) && fr[2].getFloat(v.z()) && fr[3].getFloat(v.w()))
{
fr += 4;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec4dArray")==0)
{
Vec4dArray* array = new Vec4dArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
Vec4d v;
if (fr[0].getFloat(v.x()) && fr[1].getFloat(v.y()) && fr[2].getFloat(v.z()) && fr[3].getFloat(v.w()))
{
fr += 4;
array->push_back(v);
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec2bArray")==0)
{
Vec2bArray* array = new Vec2bArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g))
{
fr+=2;
array->push_back(osg::Vec2b(r,g));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec3bArray")==0)
{
Vec3bArray* array = new Vec3bArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g,b;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g) &&
fr[2].getUInt(b))
{
fr+=3;
array->push_back(osg::Vec3b(r,g,b));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec4bArray")==0)
{
Vec4bArray* array = new Vec4bArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g,b,a;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g) &&
fr[2].getUInt(b) &&
fr[3].getUInt(a))
{
fr+=4;
array->push_back(osg::Vec4b(r,g,b,a));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec2sArray")==0)
{
Vec2sArray* array = new Vec2sArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g))
{
fr+=2;
array->push_back(osg::Vec2s(r,g));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec3sArray")==0)
{
Vec3sArray* array = new Vec3sArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g,b;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g) &&
fr[2].getUInt(b))
{
fr+=3;
array->push_back(osg::Vec3s(r,g,b));
}
else ++fr;
}
++fr;
return_array = array;
}
else if (strcmp(arrayName,"Vec4sArray")==0)
{
Vec4sArray* array = new Vec4sArray;
array->reserve(capacity);
while (!fr.eof() && fr[0].getNoNestedBrackets()>entry)
{
unsigned int r,g,b,a;
if (fr[0].getUInt(r) &&
fr[1].getUInt(g) &&
fr[2].getUInt(b) &&
fr[3].getUInt(a))
{
fr+=4;
array->push_back(osg::Vec4s(r,g,b,a));
}
else ++fr;
}
++fr;
return_array = array;
}
if (return_array)
{
if (!uniqueID.empty()) fr.registerUniqueIDForObject(uniqueID.c_str(),return_array);
}
return return_array;
}
/** Make a rotation Quat which will rotate vec1 to vec2
This routine uses only fast geometric transforms, without costly acos/sin computations.
It's exact, fast, and with less degenerate cases than the acos/sin method.
For an explanation of the math used, you may see for example:
http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
@note This is the rotation with shortest angle, which is the one equivalent to the
acos/sin transform method. Other rotations exists, for example to additionally keep
a local horizontal attitude.
@author Nicolas Brodu
*/
void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{
// This routine takes any vector as argument but normalized
// vectors are necessary, if only for computing the dot product.
// Too bad the API is that generic, it leads to performance loss.
// Even in the case the 2 vectors are not normalized but same length,
// the sqrt could be shared, but we have no way to know beforehand
// at this point, while the caller may know.
// So, we have to test... in the hope of saving at least a sqrt
Vec3d sourceVector = from;
Vec3d targetVector = to;
value_type fromLen2 = from.length2();
value_type fromLen;
// normalize only when necessary, epsilon test
if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
fromLen = sqrt(fromLen2);
sourceVector /= fromLen;
} else fromLen = 1.0;
value_type toLen2 = to.length2();
// normalize only when necessary, epsilon test
if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
value_type toLen;
// re-use fromLen for case of mapping 2 vectors of the same length
if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
toLen = fromLen;
}
else toLen = sqrt(toLen2);
targetVector /= toLen;
}
// Now let's get into the real stuff
// Use "dot product plus one" as test as it can be re-used later on
double dotProdPlus1 = 1.0 + sourceVector * targetVector;
// Check for degenerate case of full u-turn. Use epsilon for detection
if (dotProdPlus1 < 1e-7) {
// Get an orthogonal vector of the given vector
// in a plane with maximum vector coordinates.
// Then use it as quaternion axis with pi angle
// Trick is to realize one value at least is >0.6 for a normalized vector.
if (fabs(sourceVector.x()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x());
_v[0] = 0.0;
_v[1] = sourceVector.z() / norm;
_v[2] = -sourceVector.y() / norm;
_v[3] = 0.0;
} else if (fabs(sourceVector.y()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y());
_v[0] = -sourceVector.z() / norm;
_v[1] = 0.0;
_v[2] = sourceVector.x() / norm;
_v[3] = 0.0;
} else {
const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z());
_v[0] = sourceVector.y() / norm;
_v[1] = -sourceVector.x() / norm;
_v[2] = 0.0;
_v[3] = 0.0;
}
}
else {
// Find the shortest angle quaternion that transforms normalized vectors
// into one other. Formula is still valid when vectors are colinear
const double s = sqrt(0.5 * dotProdPlus1);
const Vec3d tmp = sourceVector ^ targetVector / (2.0*s);
_v[0] = tmp.x();
_v[1] = tmp.y();
_v[2] = tmp.z();
_v[3] = s;
}
}
bool FlightManipulator::performMovement()
{
// return if less then two events have been added.
if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;
double eventTimeDelta = _ga_t0->getTime()-_ga_t1->getTime();
if (eventTimeDelta<0.0f)
{
OSG_WARN << "Manipulator warning: eventTimeDelta = " << eventTimeDelta << std::endl;
eventTimeDelta = 0.0f;
}
unsigned int buttonMask = _ga_t1->getButtonMask();
if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON)
{
performMovementLeftMouseButton(eventTimeDelta, 0., 0.);
}
else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON|GUIEventAdapter::RIGHT_MOUSE_BUTTON))
{
performMovementMiddleMouseButton(eventTimeDelta, 0., 0.);
}
else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON)
{
performMovementRightMouseButton(eventTimeDelta, 0., 0.);
}
float dx = _ga_t0->getXnormalized();
float dy = _ga_t0->getYnormalized();
CoordinateFrame cf=getCoordinateFrame(_eye);
Matrixd rotation_matrix;
rotation_matrix.makeRotate(_rotation);
Vec3d up = Vec3d(0.0,1.0,0.0) * rotation_matrix;
Vec3d lv = Vec3d(0.0,0.0,-1.0) * rotation_matrix;
Vec3d sv = lv^up;
sv.normalize();
double pitch = -inDegrees(dy*50.0f*eventTimeDelta);
double roll = inDegrees(dx*50.0f*eventTimeDelta);
Quat delta_rotate;
Quat roll_rotate;
Quat pitch_rotate;
pitch_rotate.makeRotate(pitch,sv.x(),sv.y(),sv.z());
roll_rotate.makeRotate(roll,lv.x(),lv.y(),lv.z());
delta_rotate = pitch_rotate*roll_rotate;
if (_yawMode==YAW_AUTOMATICALLY_WHEN_BANKED)
{
//float bank = asinf(sv.z());
double bank = asinf(sv *getUpVector(cf));
double yaw = inRadians(bank)*eventTimeDelta;
Quat yaw_rotate;
//yaw_rotate.makeRotate(yaw,0.0f,0.0f,1.0f);
yaw_rotate.makeRotate(yaw,getUpVector(cf));
delta_rotate = delta_rotate*yaw_rotate;
}
lv *= (_velocity*eventTimeDelta);
_eye += lv;
_rotation = _rotation*delta_rotate;
return true;
}
void Camera::renderGraph(const Node *graph)
{
if (!graph || dynamic_cast<const osg::Camera*>(graph))
return;
const osg::BoundingSphered &bound = computeBoundsOf(graph);
const double r = bound.radius();
const Vec3d view_center = bound.center() * qMatrix.back();
if (-view_center.z() + r < znear || -view_center.z() - r > zfar)
return;
bool bPopMatrix = false;
const osg::StateSet *stateset = graph->getStateSet();
if (stateset)
{
qStateSet.push_back(qStateSet.back());
processStateSet(stateset, qStateSet.back());
}
const PositionAttitudeTransform *ptrans = dynamic_cast<const PositionAttitudeTransform*>(graph);
if (ptrans)
{
const Matrixd mat = Matrixd::translate(-ptrans->getPivotPoint())
* Matrixd::rotate(ptrans->getAttitude())
* Matrixd::scale(ptrans->getScale())
* Matrixd::translate(ptrans->getPosition());
pushMatrix(mat);
bPopMatrix = true;
}
const MatrixTransform *mtrans = dynamic_cast<const MatrixTransform*>(graph);
if (mtrans)
{
pushMatrix(mtrans->getMatrix());
bPopMatrix = true;
}
const Switch* sgroup = dynamic_cast<const Switch*>(graph);
if (sgroup)
{
for(unsigned int i = 0, nb = sgroup->getNumChildren() ; i < nb ; ++i)
if (sgroup->getValue(i))
renderGraph(sgroup->getChild(i));
}
else
{
const Group* group = dynamic_cast<const Group*>(graph);
if (group)
{
for(unsigned int i = 0, nb = group->getNumChildren() ; i < nb ; ++i)
renderGraph(group->getChild(i));
}
}
const Geode *geode = dynamic_cast<const Geode*>(graph);
if (geode)
{
glEnableClientState(GL_VERTEX_ARRAY);
CHECK_GL();
bool bParentStateApplied = false;
for(unsigned int i = 0, nb = geode->getNumDrawables() ; i < nb ; ++i)
{
const Geometry * const geom = geode->getDrawable(i)->asGeometry();
if (!geom)
{
LOG_ERROR() << "[Camera] unsupported drawable : " << geode->getDrawable(i)->className() << std::endl;
continue;
}
const Array * const vtx = geom->getVertexArray();
const Array * const normals = geom->getNormalArray();
const Array * const tcoord = geom->getTexCoordArray(0);
if (!vtx)
{
LOG_ERROR() << "[Camera] no vertex array in Geometry object" << std::endl;
continue;
}
StateSet *local_state = NULL;
char tmp[sizeof(StateSet)]; //! Avoid dynamic allocation
const osg::StateSet *geom_state = geom->getStateSet();
if (geom_state)
{
local_state = new(tmp) StateSet(qStateSet.back());
processStateSet(geom_state, *local_state);
}
if (local_state)
{
local_state->apply();
bParentStateApplied = false;
}
else if (!bParentStateApplied)
{
bParentStateApplied = true;
qStateSet.back().apply();
}
if (bDoublePrecisionMode)
{
if (processed_vertex_array.size() < vtx->getNumElements())
processed_vertex_array.resize(vtx->getNumElements());
const void * const ptr = vtx->getDataPointer();
const osg::Matrixd &mat = qMatrix.back() * projectionMatrix;
switch(vtx->getDataType())
{
case GL_FLOAT:
if (vtx->getDataSize() == 4)
{
const size_t nb = vtx->getNumElements();
for(size_t i = 0 ; i < nb ; ++i)
{
const Vec4d &p = Vec4d(((const Vec4f*)ptr)[i]) * mat;
processed_vertex_array[i] = p / p.w();
}
}
else if (vtx->getDataSize() == 3)
{
const size_t nb = vtx->getNumElements();
for(size_t i = 0 ; i < nb ; ++i)
{
const Vec4d &p = Vec4d(((const Vec3f*)ptr)[i], 1.0) * mat;
processed_vertex_array[i] = p / p.w();
}
}
break;
}
glEnableClientState(GL_COLOR_ARRAY);
CHECK_GL();
if (pContext->hasVBOSupport())
{
glBindBuffer(GL_ARRAY_BUFFER, 0);
CHECK_GL();
}
glColorPointer(4, GL_FLOAT, 0, &(processed_vertex_array.front()));
CHECK_GL();
}
if (pContext->hasVBOSupport())
{
glBindBuffer(GL_ARRAY_BUFFER, pContext->getDataVBOID(vtx->getDataPointer(), vtx->getTotalDataSize()));
CHECK_GL();
glVertexPointer(vtx->getDataSize(), vtx->getDataType(), 0, 0);
CHECK_GL();
if (normals)
{
glEnableClientState(GL_NORMAL_ARRAY);
CHECK_GL();
glBindBuffer(GL_ARRAY_BUFFER, pContext->getDataVBOID(normals->getDataPointer(), normals->getTotalDataSize()));
CHECK_GL();
glNormalPointer(normals->getDataType(), 0, 0);
CHECK_GL();
}
else
{
glDisableClientState(GL_NORMAL_ARRAY);
CHECK_GL();
}
if (tcoord)
{
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
CHECK_GL();
glBindBuffer(GL_ARRAY_BUFFER, pContext->getDataVBOID(tcoord->getDataPointer(), tcoord->getTotalDataSize()));
CHECK_GL();
glTexCoordPointer(tcoord->getDataSize(), tcoord->getDataType(), 0, 0);
CHECK_GL();
}
else
{
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
CHECK_GL();
}
const size_t nb_prim_set = geom->getNumPrimitiveSets();
for(size_t j = 0 ; j < nb_prim_set ; ++j)
{
const PrimitiveSet * const pset = geom->getPrimitiveSet(j);
const DrawElements * const elts = pset->getDrawElements();
switch(pset->getType())
{
case DrawElements::PrimitiveType:
break;
case DrawElements::DrawArraysPrimitiveType:
glDrawArrays(pset->getMode(), pset->index(0), pset->getNumIndices());
CHECK_GL();
break;
case DrawElements::DrawArrayLengthsPrimitiveType: break;
case DrawElements::DrawElementsUBytePrimitiveType:
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, pContext->getIndexVBOID(elts->getDataPointer(), elts->getTotalDataSize()));
CHECK_GL();
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_BYTE, 0);
CHECK_GL();
break;
case DrawElements::DrawElementsUShortPrimitiveType:
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, pContext->getIndexVBOID(elts->getDataPointer(), elts->getTotalDataSize()));
CHECK_GL();
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_SHORT, 0);
CHECK_GL();
break;
case DrawElements::DrawElementsUIntPrimitiveType:
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, pContext->getIndexVBOID(elts->getDataPointer(), elts->getTotalDataSize()));
CHECK_GL();
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_INT, 0);
CHECK_GL();
break;
}
}
}
else
{
glVertexPointer(vtx->getDataSize(), vtx->getDataType(), 0, vtx->getDataPointer());
CHECK_GL();
if (normals)
{
glEnableClientState(GL_NORMAL_ARRAY);
CHECK_GL();
glNormalPointer(normals->getDataType(), 0, normals->getDataPointer());
CHECK_GL();
}
else
{
glDisableClientState(GL_NORMAL_ARRAY);
CHECK_GL();
}
if (tcoord)
{
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
CHECK_GL();
glTexCoordPointer(tcoord->getDataSize(), tcoord->getDataType(), 0, tcoord->getDataPointer());
CHECK_GL();
}
else
{
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
CHECK_GL();
}
const size_t nb_prim_set = geom->getNumPrimitiveSets();
for(size_t j = 0 ; j < nb_prim_set ; ++j)
{
const PrimitiveSet * const pset = geom->getPrimitiveSet(j);
const DrawElements * const elts = pset->getDrawElements();
switch(pset->getType())
{
case DrawElements::PrimitiveType:
break;
case DrawElements::DrawArraysPrimitiveType:
glDrawArrays(pset->getMode(), pset->index(0), pset->getNumIndices());
CHECK_GL();
break;
case DrawElements::DrawArrayLengthsPrimitiveType: break;
case DrawElements::DrawElementsUBytePrimitiveType:
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_BYTE, elts->getDataPointer());
CHECK_GL();
break;
case DrawElements::DrawElementsUShortPrimitiveType:
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_SHORT, elts->getDataPointer());
CHECK_GL();
break;
case DrawElements::DrawElementsUIntPrimitiveType:
glDrawRangeElements(elts->getMode(), 0, vtx->getNumElements() - 1, elts->getNumIndices(), GL_UNSIGNED_INT, elts->getDataPointer());
CHECK_GL();
break;
}
}
}
if (local_state)
local_state->~StateSet();
}
}
if (bPopMatrix)
popMatrix();
if (stateset)
qStateSet.pop_back();
}
//--------------------------------------------------------------------------------------------------
/// Create a sphere with center in origin
///
/// \param radius Radius of sphere
/// \param numSlices The number of subdivisions around the z-axis (similar to lines of longitude).
/// \param numStacks The number of subdivisions along the z-axis (similar to lines of latitude).
/// \param builder Geometry builder to use when creating geometry
//--------------------------------------------------------------------------------------------------
void GeometryUtils::createSphere(double radius, uint numSlices, uint numStacks, GeometryBuilder* builder)
{
// Code is strongly inspired by mesa.
// From GLviewAPI:
// float nsign = bNormalsOutwards ? 1.0f : -1.0f;
// Could be added as a param if needed (e.g. dome)
const double nsign = 1.0;
double rho = PI_D/numStacks;
double theta = 2.0*PI_D/static_cast<double>(numSlices);
// Array to receive the node coordinates
Vec3fArray vertices;
uint vertexCount = 1 + 2*numSlices + (numStacks - 2)*numSlices;
vertices.reserve(vertexCount);
// Create the +Z end as triangles
Vec3d vTop(0.0, 0.0, nsign*radius);
vertices.add(Vec3f(vTop));
ref<UIntArray> triangleFan = new UIntArray;
triangleFan->reserve(numSlices + 2);
triangleFan->add(0);
uint j;
for (j = 0; j < numSlices; j++)
{
double localTheta = j * theta;
Vec3d v;
v.x() = -Math::sin(localTheta) * Math::sin(rho);
v.y() = Math::cos(localTheta) * Math::sin(rho);
v.z() = nsign * Math::cos(rho);
v *= radius;
vertices.add(Vec3f(v));
triangleFan->add(j + 1);
}
// Close top fan
triangleFan->add(1);
builder->addTriangleFan(*triangleFan);
// Intermediate stacks as quad-strips
// First and last stacks are handled separately
ref<UIntArray> quadStrip = new UIntArray;
quadStrip->reserve(numSlices*2 + 2);
uint i;
for (i = 1; i < numStacks - 1; i++)
{
double localRho = i * rho;
quadStrip->setSizeZero();
for (j = 0; j < numSlices; j++)
{
double localTheta = j * theta;
Vec3d v;
v.x() = -Math::sin(localTheta) * Math::sin(localRho + rho);
v.y() = Math::cos(localTheta) * Math::sin(localRho + rho);
v.z() = nsign * Math::cos(localRho + rho);
v *= radius;
vertices.add(Vec3f(v));
uint iC1 = (i*numSlices) + 1 + j;
uint iC0 = iC1 - numSlices;
quadStrip->add(iC0);
quadStrip->add(iC1);
}
// Close quad-strip
uint iStartC1 = (i*numSlices) + 1;
uint iStartC0 = iStartC1 - numSlices;
quadStrip->add(iStartC0);
quadStrip->add(iStartC1);
builder->addQuadStrip(*quadStrip);
}
// Create -Z end as triangles
Vec3d vBot( 0.0, 0.0, -radius*nsign );
vertices.add(Vec3f(vBot));
uint endNodeIndex = static_cast<uint>(vertices.size()) - 1;
triangleFan->setSizeZero();
triangleFan->add(endNodeIndex);
for (j = 0; j < numSlices; j++)
{
triangleFan->add(endNodeIndex - j - 1);
}
// Close bottom fan
triangleFan->add(endNodeIndex - 1);
builder->addTriangleFan(*triangleFan);
builder->addVertices(vertices);
}
void OsgViewerBase::setSunLightPosition( const Vec3d& position )
{
_sunLight->setPosition(Vec4(position.x(), position.y(), position.z(), 0.0f));
}
void SimpleDraw::PointArrowAt( Vec3d point, float radius /*= 1.0f*/ )
{
DrawArrowDirected(point, point + (double(radius) * Vec3d (Max(0.2, point.x()),point.y(),point.z()).normalized()), radius, false);
}
