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);
}
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()));
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
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
}
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);
}
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
String PropertyXmlSerializer::valueTextFromVec3dVariant(const Variant& variant)
{
CVF_ASSERT(variant.type() == Variant::VEC3D);
Vec3d val = variant.getVec3d();
String txt = String::number(val.x()) + " " + String::number(val.y()) + " " + String::number(val.z());
return txt;
}
//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
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;
}
//--------------------------------------------------------------------------------------------------
/// 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();
}
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;
}
//--------------------------------------------------------------------------------------------------
/// Generate surface representation of the specified cut plane
///
/// \note Will compute normals before returning geometry
//--------------------------------------------------------------------------------------------------
void StructGridCutPlane::computeCutPlane()
{
DebugTimer tim("StructGridCutPlane::computeCutPlane", DebugTimer::DISABLED);
bool doMapScalar = false;
if (m_mapScalarSetIndex != UNDEFINED_UINT && m_scalarMapper.notNull())
{
doMapScalar = true;
}
uint cellCountI = m_grid->cellCountI();
uint cellCountJ = m_grid->cellCountJ();
uint cellCountK = m_grid->cellCountK();
// Clear any current data
m_vertices.clear();
m_vertexScalars.clear();
m_triangleIndices.clear();
m_meshLineIndices.clear();
// The indexing conventions for vertices and
// edges used in the algorithm:
// edg verts
// 4-------------5 *------4------* 0 0 - 1
// /| /| /| /| 1 1 - 2
// / | / | 7/ | 5/ | 2 2 - 3
// / | / | |z / 8 / 9 3 3 - 0
// 7-------------6 | | /y *------6------* | 4 4 - 5
// | | | | |/ | | | | 5 5 - 6
// | 0---------|---1 *---x | *------0--|---* 6 6 - 7
// | / | / 11 / 10 / 7 7 - 4
// | / | / | /3 | /1 8 0 - 4
// |/ |/ |/ |/ 9 1 - 5
// 3-------------2 *------2------* 10 2 - 6
// vertex indices edge indices 11 3 - 7
//
uint k;
for (k = 0; k < cellCountK; k++)
{
uint j;
for (j = 0; j < cellCountJ; j++)
{
uint i;
for (i = 0; i < cellCountI; i++)
{
size_t cellIndex = m_grid->cellIndexFromIJK(i, j, k);
Vec3d minCoord;
Vec3d maxCoord;
m_grid->cellMinMaxCordinates(cellIndex, &minCoord, &maxCoord);
// Early reject for cells outside clipping box
if (m_clippingBoundingBox.isValid())
{
BoundingBox cellBB(minCoord, maxCoord);
if (!m_clippingBoundingBox.intersects(cellBB))
{
continue;
}
}
// Check if plane intersects this cell and skip if it doesn't
if (!isCellIntersectedByPlane(m_plane, minCoord, maxCoord))
{
continue;
}
GridCell cell;
bool isClipped = false;
if (m_clippingBoundingBox.isValid())
{
if (!m_clippingBoundingBox.contains(minCoord) || !m_clippingBoundingBox.contains(maxCoord))
{
isClipped = true;
minCoord.x() = CVF_MAX(minCoord.x(), m_clippingBoundingBox.min().x());
minCoord.y() = CVF_MAX(minCoord.y(), m_clippingBoundingBox.min().y());
minCoord.z() = CVF_MAX(minCoord.z(), m_clippingBoundingBox.min().z());
maxCoord.x() = CVF_MIN(maxCoord.x(), m_clippingBoundingBox.max().x());
maxCoord.y() = CVF_MIN(maxCoord.y(), m_clippingBoundingBox.max().y());
maxCoord.z() = CVF_MIN(maxCoord.z(), m_clippingBoundingBox.max().z());
}
}
cell.p[0].set(minCoord.x(), maxCoord.y(), minCoord.z());
cell.p[1].set(maxCoord.x(), maxCoord.y(), minCoord.z());
cell.p[2].set(maxCoord.x(), minCoord.y(), minCoord.z());
cell.p[3].set(minCoord.x(), minCoord.y(), minCoord.z());
cell.p[4].set(minCoord.x(), maxCoord.y(), maxCoord.z());
cell.p[5].set(maxCoord.x(), maxCoord.y(), maxCoord.z());
cell.p[6].set(maxCoord.x(), minCoord.y(), maxCoord.z());
cell.p[7].set(minCoord.x(), minCoord.y(), maxCoord.z());
// Fetch scalar values
double cellScalarValue = 0;
if (doMapScalar)
{
cellScalarValue = m_grid->cellScalar(m_mapScalarSetIndex, i, j, k);
// If we're doing node averaging we must populate grid cell with scalar values interpolated to the grid points
if (m_mapNodeAveragedScalars)
{
if (isClipped)
{
double scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[0], &scalarVal)) cell.s[0] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[1], &scalarVal)) cell.s[1] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[2], &scalarVal)) cell.s[2] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[3], &scalarVal)) cell.s[3] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[4], &scalarVal)) cell.s[4] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[5], &scalarVal)) cell.s[5] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[6], &scalarVal)) cell.s[6] = scalarVal;
if (m_grid->pointScalar(m_mapScalarSetIndex, cell.p[7], &scalarVal)) cell.s[7] = scalarVal;
}
else
{
cell.s[0] = m_grid->gridPointScalar(m_mapScalarSetIndex, i, j + 1, k);
cell.s[1] = m_grid->gridPointScalar(m_mapScalarSetIndex, i + 1, j + 1, k);
cell.s[2] = m_grid->gridPointScalar(m_mapScalarSetIndex, i + 1, j, k);
cell.s[3] = m_grid->gridPointScalar(m_mapScalarSetIndex, i, j, k);
cell.s[4] = m_grid->gridPointScalar(m_mapScalarSetIndex, i, j + 1, k + 1);
cell.s[5] = m_grid->gridPointScalar(m_mapScalarSetIndex, i + 1, j + 1, k + 1);
cell.s[6] = m_grid->gridPointScalar(m_mapScalarSetIndex, i + 1, j, k + 1);
cell.s[7] = m_grid->gridPointScalar(m_mapScalarSetIndex, i, j, k + 1);
}
}
}
Triangles triangles;
uint numTriangles = polygonise(m_plane, cell, &triangles);
if (numTriangles > 0)
{
// Add all the referenced vertices
// At the same time registering their index in the 'global' vertex list
uint globalVertexIndices[12];
int iv;
for (iv = 0; iv < 12; iv++)
{
if (triangles.usedVertices[iv])
{
globalVertexIndices[iv] = static_cast<uint>(m_vertices.size());
m_vertices.push_back(Vec3f(triangles.vertices[iv]));
if (doMapScalar)
{
if (m_mapNodeAveragedScalars)
{
m_vertexScalars.push_back(triangles.scalars[iv]);
}
else
{
m_vertexScalars.push_back(cellScalarValue);
}
}
}
else
{
globalVertexIndices[iv] = UNDEFINED_UINT;
}
}
// Build triangles from the cell
const size_t prevNumTriangleIndices = m_triangleIndices.size();
uint t;
for (t = 0; t < numTriangles; t++)
{
m_triangleIndices.push_back(globalVertexIndices[triangles.triangleIndices[3*t]]);
m_triangleIndices.push_back(globalVertexIndices[triangles.triangleIndices[3*t + 1]]);
m_triangleIndices.push_back(globalVertexIndices[triangles.triangleIndices[3*t + 2]]);
}
// Add mesh line indices
addMeshLineIndices(&m_triangleIndices[prevNumTriangleIndices], numTriangles);
}
}
}
}
//Trace::show("Vertices:%d TriConns:%d Tris:%d", m_vertices.size(), m_triangleIndices.size(), m_triangleIndices.size()/3);
tim.reportTimeMS();
}
/** 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;
}
//--------------------------------------------------------------------------------------------------
/// 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);
}
