double MultitouchNavigation::angleBetween3DVectors(osg::Vec3 v1, osg::Vec3 v2)
{
// http://codered.sat.qc.ca/redmine/projects/spinframework/repository/revisions/b6245189c19a7c6ba4fdb126940321c41c44e228/raw/src/spin/osgUtil.cpp
// normalize vectors (note: this must be done alone, not within any vector arithmetic. why?!)
v1.normalize();
v2.normalize();
// Get the dot product of the vectors
double dotProduct = v1 * v2;
// for acos, the value has to be between -1.0 and 1.0, but due to numerical imprecisions it sometimes comes outside this range
if (dotProduct > 1.0)
dotProduct = 1.0;
if (dotProduct < -1.0)
dotProduct = -1.0;
// Get the angle in radians between the 2 vectors (should this be -acos ? ie, negative?)
double angle = acos(dotProduct);
// Here we make sure that the angle is not a -1.#IND0000000 number, which means indefinite
if (std::isnan(angle)) //__isnand(x)
return 0;
// Return the angle in radians
return (angle);
}
bool
HeightFieldUtils::getNormalAtNormalizedLocation(const osg::HeightField* input,
double nx, double ny,
osg::Vec3& output,
ElevationInterpolation interp)
{
double xcells = (double)(input->getNumColumns()-1);
double ycells = (double)(input->getNumRows()-1);
double w = input->getXInterval() * xcells * 111000.0;
double h = input->getYInterval() * ycells * 111000.0;
double ndx = 1.0/xcells;
double ndy = 1.0/ycells;
double xmin = osg::clampAbove( nx-ndx, 0.0 );
double xmax = osg::clampBelow( nx+ndx, 1.0 );
double ymin = osg::clampAbove( ny-ndy, 0.0 );
double ymax = osg::clampBelow( ny+ndy, 1.0 );
osg::Vec3 west (xmin*w, ny*h, getHeightAtNormalizedLocation(input, xmin, ny, interp));
osg::Vec3 east (xmax*w, ny*h, getHeightAtNormalizedLocation(input, xmax, ny, interp));
osg::Vec3 south(nx*w, ymin*h, getHeightAtNormalizedLocation(input, nx, ymin, interp));
osg::Vec3 north(nx*w, ymax*h, getHeightAtNormalizedLocation(input, nx, ymax, interp));
output = (west-east) ^ (north-south);
output.normalize();
return true;
}
// -------------------------------------------------------------------------
void Hermite::getTangentOnSegment(osg::Vec3& tangent, double distance, unsigned int segment)
{
float fH1 = ( 6.0f * distance - 6.0f) * distance;
float fH2 = ( 3.0f * distance - 4.0f) * distance + 1.0f;
float fH3 = ( 3.0f * distance - 2.0f) * distance;
float fH4 = (-6.0f * distance + 6.0f) * distance;
const ControlPoint& begin = (*_segments)[segment].getBegin();
const ControlPoint& end = (*_segments)[segment].getEnd();
tangent = begin.position * fH1 + begin.tangent * fH2 + end.tangent * fH3 + end.position * fH4;
tangent.normalize();
/*
tangent[0] = begin->position.x * fH1 +
begin->tangent.x * fH2 +
end->tangent.x * fH3 +
end->position.x * fH4;
tangent[1] = begin->position.y * fH1 +
begin->tangent.y * fH2 +
end->tangent.y * fH3 +
end->position.y * fH4;
tangent[2] = begin->position.z * fH1 +
begin->tangent.z * fH2 +
end->tangent.z * fH3 +
end->position.z * fH4;
Math::normalize(tangent);
*/
}
void normal(osg::Vec3 v1, osg::Vec3 v2, osg::Vec3 v3)
{
osg::Vec3 u,v;
// right hand system, CCW triangle
u = v2 - v1;
v = v3 - v1;
triNormal = u^v;
triNormal.normalize();
}
/**
* Retrieves the normal of the terrain at the specified location.
*
* @param x the x coordinate to check
* @param y the y coordinate to check
* @param normal the location at which to store the normal
* instead of triangle mesh height
*/
void SurfaceObject::GetNormal(float x, float y, osg::Vec3& normal)
{
float z = GetHeight(x, y);
osg::Vec3 v1(0.1f, 0.0f, GetHeight(x + 0.1f, y) - z);
osg::Vec3 v2(0.0f, 0.1f, GetHeight(x, y + 0.1f) - z );
normal = v1 ^ v2;
normal.normalize();
/* if (normal.x() != 0 || normal.y() != 0 || normal.z() != 1 )
std::cout<<"NORM "<<"("<<normal.x()<<",\t"<<normal.y()<<",\t"<<normal.z()<<")"<<std::endl;
*/
}
bool
TileModel::ElevationData::getNormal(const osg::Vec3d& ndc,
const GeoLocator* ndcLocator,
osg::Vec3& output,
ElevationInterpolation interp ) const
{
if ( !_locator.valid() || !ndcLocator )
{
output.set(0,0,1);
return false;
}
double xcells = (double)(_hf->getNumColumns()-1);
double ycells = (double)(_hf->getNumRows()-1);
double xres = 1.0/xcells;
double yres = 1.0/ycells;
osg::Vec3d hf_ndc;
GeoLocator::convertLocalCoordBetween( *ndcLocator, ndc, *_locator.get(), hf_ndc );
float centerHeight = HeightFieldUtils::getHeightAtNormalizedLocation(_hf.get(), hf_ndc.x(), hf_ndc.y(), interp);
osg::Vec3d west ( hf_ndc.x()-xres, hf_ndc.y(), 0.0 );
osg::Vec3d east ( hf_ndc.x()+xres, hf_ndc.y(), 0.0 );
osg::Vec3d south( hf_ndc.x(), hf_ndc.y()-yres, 0.0 );
osg::Vec3d north( hf_ndc.x(), hf_ndc.y()+yres, 0.0 );
if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, west.x(), west.y(), west.z(), interp))
west.z() = centerHeight;
if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, east.x(), east.y(), east.z(), interp))
east.z() = centerHeight;
if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, south.x(), south.y(), south.z(), interp))
south.z() = centerHeight;
if (!HeightFieldUtils::getHeightAtNormalizedLocation(_neighbors, north.x(), north.y(), north.z(), interp))
north.z() = centerHeight;
osg::Vec3d westWorld, eastWorld, southWorld, northWorld;
_locator->unitToModel(west, westWorld);
_locator->unitToModel(east, eastWorld);
_locator->unitToModel(south, southWorld);
_locator->unitToModel(north, northWorld);
output = (eastWorld-westWorld) ^ (northWorld-southWorld);
output.normalize();
return true;
}
/*
* Ok, simulate a track-ball. Project the points onto the virtual
* trackball, then figure out the axis of rotation, which is the cross
* product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
* Note: This is a deformed trackball-- is a trackball in the center,
* but is deformed into a hyperbolic sheet of rotation away from the
* center. This particular function was chosen after trying out
* several variations.
*
* It is assumed that the arguments to this routine are in the range
* (-1.0 ... 1.0)
*/
void Vwr::CameraManipulator::trackball(osg::Vec3& axis, float& angle,
float p1x, float p1y, float p2x, float p2y) {
/*
* First, figure out z-coordinates for projection of P1 and P2 to
* deformed sphere
*/
osg::Matrix rotation_matrix(_rotation);
osg::Vec3 uv = osg::Vec3(0.0f, 1.0f, 0.0f) * rotation_matrix;
osg::Vec3 sv = osg::Vec3(1.0f, 0.0f, 0.0f) * rotation_matrix;
osg::Vec3 lv = osg::Vec3(0.0f, 0.0f, -1.0f) * rotation_matrix;
osg::Vec3 p1 = sv * p1x + uv * p1y - lv * tb_project_to_sphere(
_trackballSize, p1x, p1y);
osg::Vec3 p2 = sv * p2x + uv * p2y - lv * tb_project_to_sphere(
_trackballSize, p2x, p2y);
/*
* Now, we want the cross product of P1 and P2
*/
// Robert,
//
// This was the quick 'n' dirty fix to get the trackball doing the right
// thing after fixing the Quat rotations to be right-handed. You may want
// to do something more elegant.
// axis = p1^p2;
axis = p2 ^ p1;
axis.normalize();
/*
* Figure out how much to rotate around that axis.
*/
float t = (p2 - p1).length() / (2.0 * _trackballSize);
/*
* Avoid problems with out-of-control values...
*/
if (t > 1.0)
t = 1.0;
if (t < -1.0)
t = -1.0;
angle = inRadians(asin(t));
}
void dtAnim::GetCelestialCoordinates(osg::Vec3 target,
const osg::Vec3 &lookForward,
float &azimuth,
float &elevation)
{
target.normalize();
// Derive the reference frame for the "look" pose
osg::Vec3 frameRight = lookForward ^ osg::Z_AXIS;
osg::Vec3 frameUp = frameRight ^ lookForward;
osg::Matrix frameMatrix(frameRight.x(), frameRight.y(), frameRight.z(), 0.0f,
lookForward.x(), lookForward.y(), lookForward.z(), 0.0f,
frameUp.x(), frameUp.y(), frameUp.z(), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
// Transform the target into the proper coordinate frame
target = frameMatrix * target;
// Project our target vector onto the xy plane
// in order to calculate azimuth
osg::Vec3f targetRight = target ^ osg::Z_AXIS;
targetRight.normalize();
osg::Vec3f targetForward = osg::Z_AXIS ^ targetRight;
targetForward.normalize();
// Use the projected vector to calculate the azimuth
// between the projection and the character forward
float lookDotTargetForward = targetForward * osg::Y_AXIS;
float targetDotTargetForward = targetForward * target;
// acos needs it's parameter between -1 and 1
dtUtil::Clamp(lookDotTargetForward, -1.0f, 1.0f);
dtUtil::Clamp(targetDotTargetForward, -1.0f, 1.0f);
// We need to manually determine the sign
float azimuthSign = ((target * osg::X_AXIS) > 0.0f) ? -1.0f: 1.0f;
float elevationSign = ((target * osg::Z_AXIS) > 0.0f) ? 1.0f: -1.0f;
// We can use the angle between the projection
// and the original target to determine elevation
elevation = acos(targetDotTargetForward) * elevationSign;
azimuth = acos(lookDotTargetForward) * azimuthSign;
}
bool
HeightFieldUtils::getNormalAtNormalizedLocation(const osg::HeightField* input,
double nx, double ny,
osg::Vec3& output,
ElevationInterpolation interp)
{
double dx = 1.0/(double)(input->getNumColumns()-1);
double dy = 1.0/(double)(input->getNumRows()-1);
double xmin = osg::clampAbove( nx-dx, 0.0 );
double xmax = osg::clampBelow( nx+dx, 1.0 );
double ymin = osg::clampAbove( ny-dx, 0.0 );
double ymax = osg::clampBelow( ny+dy, 1.0 );
osg::Vec3 west (xmin, ny, getHeightAtNormalizedLocation(input, xmin, ny, interp));
osg::Vec3 east (xmax, ny, getHeightAtNormalizedLocation(input, xmax, ny, interp));
osg::Vec3 south(nx, ymin, getHeightAtNormalizedLocation(input, nx, ymin, interp));
osg::Vec3 north(nx, ymax, getHeightAtNormalizedLocation(input, nx, ymax, interp));
output = (west-east) ^ (north-south);
output.normalize();
return true;
}
void Wire::cutWire(osg::Vec3 normal, float dist)
{
normal.normalize();
DrawArrayLengths *newPrimitives = new DrawArrayLengths(PrimitiveSet::POLYGON);
UShortArray *newIndices = new UShortArray();
int startIndex = 0;
int index = vert->getNumElements();
for (unsigned int i = 0; i < primitives->getNumPrimitives(); i++)
{
float oldd = -1;
int numInd = 0;
for (int v = 0; v < (*primitives)[i]; v++)
{
osg::Vec3 p = (*vert)[indices->index(startIndex + v)];
float d = (p - normal * dist) * normal;
/* Grundidee:
* wir laufen allen Punkte von jedem Polygon einmal
* durch; liegt der Punkte im "Verbleib" Bereich, wird
* wird er einfach kopiert, liegt er ausserhalb, dann
* wird er weggelassen
* !! Immer wenn ein Wechsel zwischen "verbleibt" und\
* und "weglassen" erfolgt, dann muss mindestens ein
* zusaetzlicher Punkt eingefuegt werden.
*/
if (v == 0 && d >= 0)
{
newIndices->push_back((*indices)[startIndex + v]);
numInd++;
}
else
{
if (d < 0)
{
if (oldd > 0)
{
osg::Vec3 b = (*vert)[(*indices)[startIndex + v - 1]];
osg::Vec3 r = p - b;
float t = (dist - normal * p) / (normal * r);
osg::Vec3 newP = p + r * t;
osg::Vec3 n1 = (*normals)[(*indices)[startIndex + v]];
osg::Vec3 n2 = (*normals)[(*indices)[startIndex + v - 1]];
osg::Vec3 newN = n1 + (n1 - n2) * t;
vert->push_back(newP);
normals->push_back(newN);
newIndices->push_back(index);
index++;
numInd++;
}
else
{
}
}
else
{
if (oldd < 0)
{
osg::Vec3 b = (*vert)[(*indices)[startIndex + v - 1]];
osg::Vec3 r = p - b;
float t = (dist - normal * p) / (normal * r);
osg::Vec3 newP = p + r * t;
osg::Vec3 n1 = (*normals)[(*indices)[startIndex + v]];
osg::Vec3 n2 = (*normals)[(*indices)[startIndex + v - 1]];
osg::Vec3 newN = n1 + (n1 - n2) * t;
vert->push_back(newP);
normals->push_back(newN);
newIndices->push_back(index);
index++;
numInd++;
}
newIndices->push_back((*indices)[startIndex + v]);
numInd++;
}
}
oldd = d;
}
osg::Vec3 p = (*vert)[indices->index(startIndex)];
float d = (p - normal * dist) * normal;
if (d < 0)
{
if (oldd > 0)
{
osg::Vec3 b = (*vert)[(*indices)[startIndex + (*primitives)[i] - 1]];
osg::Vec3 r = p - b;
float t = (dist - normal * p) / (normal * r);
osg::Vec3 newP = p + r * t;
osg::Vec3 n1 = (*normals)[(*indices)[startIndex]];
osg::Vec3 n2 = (*normals)[(*indices)[startIndex + (*primitives)[i] - 1]];
osg::Vec3 newN = n1 + (n1 - n2) * t;
vert->push_back(newP);
normals->push_back(newN);
newIndices->push_back(index);
index++;
numInd++;
}
}
else
{
if (oldd < 0)
{
osg::Vec3 b = (*vert)[(*indices)[startIndex + (*primitives)[i] - 1]];
osg::Vec3 r = p - b;
float t = (dist - normal * p) / (normal * r);
osg::Vec3 newP = p + r * t;
osg::Vec3 n1 = (*normals)[(*indices)[startIndex]];
osg::Vec3 n2 = (*normals)[(*indices)[startIndex + (*primitives)[i] - 1]];
osg::Vec3 newN = n1 + (n1 - n2) * t;
vert->push_back(newP);
normals->push_back(newN);
newIndices->push_back(index);
index++;
numInd++;
}
}
newPrimitives->push_back(numInd);
startIndex += (*primitives)[i];
}
geom->setVertexIndices(newIndices);
geom->setNormalIndices(newIndices);
geom->removePrimitiveSet(0, geom->getNumPrimitiveSets());
geom->addPrimitiveSet(newPrimitives);
primitives = newPrimitives;
indices = newIndices;
// calcColors();
geom->dirtyBound();
geom->dirtyDisplayList();
}