bool SubSurface::Subtag( const vec3d & center )
{
UpdatePolygonPnts(); // Update polygon vector
for ( int p = 0; p < ( int )m_PolyPntsVec.size(); p++ )
{
bool inPoly = PointInPolygon( vec2d( center.x(), center.y() ), m_PolyPntsVec[p] );
if ( inPoly && m_TestType() == vsp::INSIDE )
{
return true;
}
else if ( inPoly && m_TestType() == vsp::OUTSIDE )
{
return false;
}
}
if ( m_TestType() == vsp::OUTSIDE )
{
return true;
}
return false;
}
//to return projection vector
vec3d vec3d :: proj(const vec3d& vec) const
{
float m = vec.mag();
if(m == 0)
throw "invalid vector";
return vec.unit()*(operator*(vec)/m);
}
void Surf::ApplyES( vec3d uw, double t )
{
double grm1 = m_GridDensityPtr->m_GrowRatio() - 1.0;
int nmapu = m_SrcMap.size();
int nmapw = m_SrcMap[0].size();
int ibase, jbase;
double u = uw.x();
double w = uw.y();
UWtoTargetMapij( u, w, ibase, jbase );
vec3d p = m_SurfCore.CompPnt( u, w );
int iadd[] = { 0, 1, 0, 1 };
int jadd[] = { 0, 0, 1, 1 };
for( int i = 0; i < 4; i++ )
{
int itarget = ibase + iadd[i];
int jtarget = jbase + jadd[i];
if( itarget < nmapu && itarget >= 0 && jtarget < nmapw && jtarget >= 0 )
{
vec3d p2 = m_SrcMap[ itarget ][ jtarget ].m_pt;
double r = ( p2 - p ).mag();
double targetstr = t + r * grm1;
if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
{
m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
WalkMap( itarget, jtarget, itarget, jtarget );
}
}
}
}
vec3d SSLineSeg::CompPnt( VspSurf* surf, vec3d uw_pnt ) const
{
if ( !surf )
{
return vec3d();
}
double maxu = surf->GetUMax();
double maxw = surf->GetWMax();
if ( uw_pnt.x() < 0.0 )
{
uw_pnt.set_x( 0.0 );
}
else if ( uw_pnt.x() > maxu )
{
uw_pnt.set_x( maxu );
}
if ( uw_pnt.y() < 0.0 )
{
uw_pnt.set_y( 0.0 );
}
else if ( uw_pnt.y() > maxw )
{
uw_pnt.set_y( maxw );
}
return surf->CompPnt( uw_pnt.x(), uw_pnt.y() );
}
void VspCurve::Reflect( vec3d axis, double d )
{
curve_point_type a;
a << axis.x(), axis.y(), axis.z();
m_Curve.reflect( a, d );
}
//===== Offset =====//
void VspCurve::Offset( vec3d offvec )
{
curve_point_type tr;
tr << offvec.x(), offvec.y(), offvec.z();
m_Curve.translate( tr );
}
// Spherical linear interpolation between direction vectors.
// Intermediate vectors follow great circle path with constant velocity.
vec3d slerp( const vec3d& a, const vec3d& b, const double &t )
{
vec3d an = a / a.mag();
vec3d bn = b / b.mag();
double dp = dot( an, bn );
double theta = 0.0;
if ( dp >= -1.0 && dp <= 1.0 )
{
theta = acos( dp );
}
// Initialize retvec as a-direction.
vec3d retvec = an;
// If vectors are not parallel, interpolate between them.
if ( std::abs( theta ) > 1.0e-6 )
{
// Drop division by sin(theta) because .normalize() will scale
double coeff1 = sin( ( 1.0 - t ) * theta ); // implied / sin(theta)
double coeff2 = sin( t * theta ); // implied / sin(theta)
retvec = coeff1 * an + coeff2 * bn;
retvec.normalize();
}
return retvec;
}
//////////////////////////////////////////////////////////////////////
//==== IPnt Bin ====//
//////////////////////////////////////////////////////////////////////
int IPntBin::ComputeID( vec3d & pos )
{
int id = (int)(pos.x()*10000.0) +
(int)(pos.y()*10000.0) +
(int)(pos.z()*10000.0);
return id;
}
//******* Angle Between Vectors ******//
double angle(const vec3d& a, const vec3d& b)
{
double angle = dot(a, b)/(a.mag()*b.mag());
if ( angle < -1.0 ) angle = -1.0;
else if ( angle > 1.0 ) angle = 1.0;
return(acos(angle));
}
static FLOAT64 vec_angle( vec3d v1, vec3d v2 ) {
FLOAT64 len1 = v1.len();
FLOAT64 len2 = v2.len();
FLOAT64 dot = len1 * len2;
if( dot ) {
FLOAT64 res = acos(fmin(1.0-1e-8,fmax(-1.0+1e-8,v1*v2/dot)));
return res;
}
return 0.0;
}
double VspCurve::FindNearest( double &u, const vec3d &pt, const double &u0 ) const
{
double dist;
curve_point_type p;
p << pt.x(), pt.y(), pt.z();
dist = eli::geom::intersect::minimum_distance( u, m_Curve, p, u0 );
return dist;
}
//******* Cosine of Angle Between Vectors ******//
double cos_angle(const vec3d& a, const vec3d& b)
{
double angle = dot(a, b)/(a.mag()*b.mag());
if (angle < -1.0 )
return -1.0;
else if ( angle > 1.0)
return 1.0;
return angle;
}
void PlanetViewController::move(vec3d &oldp, vec3d &p)
{
oldp = oldp.normalize();
p = p.normalize();
double oldlat = safe_asin(oldp.z);
double oldlon = atan2(oldp.y, oldp.x);
double lat = safe_asin(p.z);
double lon = atan2(p.y, p.x);
x0 -= lon - oldlon;
y0 -= lat - oldlat;
y0 = max(-M_PI / 2.0, min(M_PI / 2.0, y0));
}
//******* Angle Between Vectors ******//
double angle( const vec3d& a, const vec3d& b )
{
double angle = dot( a, b ) / ( a.mag() * b.mag() );
if ( angle >= -1.0 && angle <= 1.0 )
{
return( acos( angle ) );
}
else
{
return( 0.0 );
}
}
vec4d planeByPointNormal(vec3d point, vec3d normal) {
vec4d ret;
for (int i = 0; i < 3; i++)
ret.v[i] = normal.v[i];
ret.v[3] = -point.dot(normal);
return ret;
}
double SurfCore::FindNearest( double &u, double &w, const vec3d &pt, double u0, double w0 ) const
{
double dist;
surface_point_type p;
pt.get_pnt( p );
double umn = m_Surface.get_u0();
double wmn = m_Surface.get_v0();
double umx = m_Surface.get_umax();
double wmx = m_Surface.get_vmax();
double slop = 1e-3;
if( u0 < (umn - slop) || w0 < (wmn - slop) || u0 > (umx + slop) || w0 > (wmx + slop) )
{
printf("BAD parameter in SurfCore::FindNearest! %f %f\n", u0, w0 );
assert(false);
}
if ( u0 < umn )
u0 = umn;
if ( w0 < wmn )
w0 = wmn;
if ( u0 > umx )
u0 = umx;
if ( w0 > wmx )
w0 = wmx;
dist = eli::geom::intersect::minimum_distance( u, w, m_Surface, p, u0, w0 );
return dist;
}
mat44 translationMatrix(vec3d x) {
mat44 ret;
for (int i = 0; i < 4; i++)
for (int j = 0; j < 3; j++)
ret.m[i][j] = (i == j) ? 1 : 0;
ret.setcolumn(3, x.tohc());
return ret;
}
//to return the angle between vectors in radians
double vec3d :: angle(const vec3d& vec) const
{
float this_mag = mag();
float vec_mag = vec.mag();
if(this_mag == 0 || vec_mag == 0)
throw "invalid vector";
return acos(operator*(vec)/(this_mag*vec_mag));
}
vec4d quatFromAA(vec3d axis, double angle) {
vec4d ret;
vec3d a = axis.normalize();
for (int i = 0; i < 3; i++)
ret.v[i] = a.v[i] * sin(angle / 2);
ret.v[3] = cos(angle / 2);
return ret;
}
void Mesh::drawSurface(GLenum primitive, vec3d axis, float limit, int numSteps){
if (!isValid) return;
std::vector<Face> surface;
double inc = limit/(numSteps-1);
axis.normalize();
for (unsigned u=0; u < mesh.size(); ++u){
surface.clear();
surface.push_back(mesh[u]);
for (unsigned i = 1; i < numSteps; ++i){
Face f;
for (unsigned j=0; j< mesh[u].v.size(); ++j ){
// translate all vertices
f.v.push_back(surface[i-1].v[j] + axis*inc);
}
surface.push_back(f);
}
// draw surface
glColor3f(1,1,1);
GLfloat gray[] = {1.0f, 0.5f, 0.5f, 1.0f};
glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, gray);
glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);
for (unsigned i=0; i < surface.size()-1; ++i){
Face *f0 = &surface[i];
Face *f1 = &surface[i+1];
unsigned size = f0->v.size();
glBegin(primitive);
for (unsigned j=0; j < size; ++j){
unsigned c1 = j;
unsigned c2 = (j+1)%size;
vec3d n1 = cross(f0->v[c2] - f0->v[c1], f1->v[c1] - f0->v[c1]);
vec3d n2 = cross(f1->v[c2] - f0->v[c2], f1->v[c1] - f0->v[c2]);
n1.normalize();
n2.normalize();
// Tri 1
glNormal3f(n1.x, n1.y, n1.z);
glVertex3f(f0->v[c1].x, f0->v[c1].y, f0->v[c1].z);
glVertex3f(f0->v[c2].x, f0->v[c2].y, f0->v[c2].z);
glVertex3f(f1->v[c1].x, f1->v[c1].y, f1->v[c1].z);
// Tri 2
glNormal3f(n2.x, n2.y, n2.z);
glVertex3f(f0->v[c2].x, f0->v[c2].y, f0->v[c2].z);
glVertex3f(f1->v[c2].x, f1->v[c2].y, f1->v[c2].z);
glVertex3f(f1->v[c1].x, f1->v[c1].y, f1->v[c1].z);
}
glEnd();
}
}
}
//Constructor to initialize a vector with a vector and magnitude (the new vector will share the direction of given vector)
vec3d :: vec3d(vec3d vec, float mag)
{
vec.normalize();
if(mag == 0)
throw "invalid vector";
x=mag*vec.x;
y=mag*vec.y;
z=mag*vec.z;
}
double SurfCore::FindNearest( double &u, double &w, const vec3d &pt ) const
{
double dist;
surface_point_type p;
pt.get_pnt( p );
dist = eli::geom::intersect::minimum_distance( u, w, m_Surface, p );
return dist;
}
mat4d SphericalDeformation::deformedToTangentFrame(const vec3d &deformedPt) const
{
vec3d Uz = deformedPt.normalize();
vec3d Ux = (vec3d::UNIT_Y.crossProduct(Uz)).normalize();
vec3d Uy = Uz.crossProduct(Ux);
return mat4d(Ux.x, Ux.y, Ux.z, 0.0,
Uy.x, Uy.y, Uy.z, 0.0,
Uz.x, Uz.y, Uz.z, -R,
0.0, 0.0, 0.0, 1.0);
}
double SCurve::GetTargetLen( GridDensity* grid_den, SCurve* BCurve, vec3d p, vec3d uw, double u )
{
double len = grid_den->GetBaseLen();
if( m_Surf->GetCompID() >= 0 )
len = m_Surf->InterpTargetMap( uw.x(), uw.y() );
if(BCurve){
vec3d uwB = BCurve->m_UWCrv.comp_pnt( u );
double lenB = grid_den->GetBaseLen();
if( BCurve->m_Surf->GetCompID() >= 0 )
lenB = BCurve->m_Surf->InterpTargetMap( uwB.x(), uwB.y() );
len = min( len, lenB );
}
return len;
}
void SurfCore::GetBorderCurve( const vec3d &uw0, const vec3d &uw1, Bezier_curve &crv ) const
{
int iborder = -1;
double tol = 1.0e-12;
if ( std::abs( uw0.x() - uw1.x() ) < tol ) // U const, UMIN or UMAX
{
double umid = ( m_Surface.get_umax() + m_Surface.get_u0() ) / 2.0;
if ( uw0.x() < umid )
iborder = UMIN;
else
iborder = UMAX;
}
else if ( std::abs( uw0.y() - uw1.y() ) < tol )
{
double vmid = ( m_Surface.get_vmax() + m_Surface.get_v0() ) / 2.0;
if ( uw0.y() < vmid )
iborder = WMIN;
else
iborder = WMAX;
}
if ( iborder >= UMIN )
crv = GetBorderCurve( iborder );
}
/**
Compute the normal for a polygon.
Basically, the method simply takes the first three vertices A, B, C and
computes the normal of that triangle.
However, it is checked that A!=B and B!=C, so it is possible that more than
three vertices are looked up.
\param poly Polygon index
\param[out] N Receives the resulting normal
*/
void PolyhedronGeom::computeNormal(int poly, vec3d& N)
{
VertexLoop& loop = *(*polys[poly])[0];
int size = loop.size();
int i = 2;
if (size<3)
return;
const vec3d* a = &(verts.getValue(loop[0]));
const vec3d* b = &(verts.getValue(loop[1]));
while(a==b)
{
if (i>=size)
return;
b = &(verts.getValue(loop[i]));
i++;
}
const vec3d* c = &(verts.getValue(loop[i]));
while(b==c)
{
if (i>=size)
return;
c = &(verts.getValue(loop[i]));
i++;
}
N.cross((*b)-(*a), (*c)-(*a));
try
{
N.normalize(N);
}
catch(...)
{
N.set(0,0,0);
}
}
vec2d matrixExtract(vec3d &bigMatrix, int index)
{
int i_size = bigMatrix.size();
int k_size = bigMatrix[0].size();
vec2d result(i_size,k_size,0);
for (int i = 0; i<i_size; i++)
{
for (int k=0; k<k_size; k++)
{
result[i][k] = bigMatrix[i][k][index];
}
}
return result;
}
//// Rotate a point p by angle theta around an arbitrary axis r
//// Return the rotated point.
//// Positive angles are anticlockwise looking down the axis
//// towards the origin.
//// Assume right hand coordinate system.
vec3d RotateArbAxis(vec3d & p, double theta, vec3d & r) // Radians
{
vec3d q( 0, 0, 0 );
double costheta,sintheta;
r.normalize();
costheta = cos(theta);
sintheta = sin(theta);
q[0] += (costheta + (1 - costheta) * r[0] * r[0]) * p[0];
q[0] += ((1 - costheta) * r[0] * r[1] - r[2] * sintheta) * p[1];
q[0] += ((1 - costheta) * r[0] * r[2] + r[1] * sintheta) * p[2];
q[1] += ((1 - costheta) * r[0] * r[1] + r[2] * sintheta) * p[0];
q[1] += (costheta + (1 - costheta) * r[1] * r[1]) * p[1];
q[1] += ((1 - costheta) * r[1] * r[2] - r[0] * sintheta) * p[2];
q[2] += ((1 - costheta) * r[0] * r[2] - r[1] * sintheta) * p[0];
q[2] += ((1 - costheta) * r[1] * r[2] + r[0] * sintheta) * p[1];
q[2] += (costheta + (1 - costheta) * r[2] * r[2]) * p[2];
return(q);
}
vec3d SphericalDeformation::deformedToLocal(const vec3d &deformedPt) const
{
double l = deformedPt.length();
if (deformedPt.z >= abs(deformedPt.x) && deformedPt.z >= abs(deformedPt.y)) {
return vec3d(deformedPt.x / deformedPt.z * R, deformedPt.y / deformedPt.z * R, l - R);
}
if (deformedPt.z <= -abs(deformedPt.x) && deformedPt.z <= -abs(deformedPt.y)) {
return vec3d(INFINITY, INFINITY, INFINITY);
}
if (deformedPt.y >= abs(deformedPt.x) && deformedPt.y >= abs(deformedPt.z)) {
return vec3d(deformedPt.x / deformedPt.y * R, (2.0 - deformedPt.z / deformedPt.y) * R, l - R);
}
if (deformedPt.y <= -abs(deformedPt.x) && deformedPt.y <= -abs(deformedPt.z)) {
return vec3d(-deformedPt.x / deformedPt.y * R, (-2.0 - deformedPt.z / deformedPt.y) * R, l - R);
}
if (deformedPt.x >= abs(deformedPt.y) && deformedPt.x >= abs(deformedPt.z)) {
return vec3d((2.0 - deformedPt.z / deformedPt.x) * R, deformedPt.y / deformedPt.x * R, l - R);
}
if (deformedPt.x <= -abs(deformedPt.y) && deformedPt.x <= -abs(deformedPt.z)) {
return vec3d((-2.0 - deformedPt.z / deformedPt.x) * R, -deformedPt.y / deformedPt.x * R, l - R);
}
assert(false);
return vec3d::ZERO;
}
//===== Interpolate Creates cubic spline with set end slopes ====//
void VspCurve::InterpolateCSpline( vector< vec3d > & input_pnt_vec, const vec3d &start_slope, const vec3d &end_slope, const vector<double> ¶m )
{
// do some checking of vector lengths
if ( param.size() != input_pnt_vec.size() )
{
std::cerr << "Invalid number of points and parameters in curve interpolation " << __LINE__ << std::endl;
assert( false );
return;
}
// copy points over to new type
vector<curve_point_type> pts( input_pnt_vec.size() );
curve_point_type sslope, eslope;
for ( size_t i = 0; i < pts.size(); ++i )
{
pts[i] << input_pnt_vec[i].x(), input_pnt_vec[i].y(), input_pnt_vec[i].z();
}
sslope << start_slope.x(), start_slope.y(), start_slope.z();
eslope << end_slope.x(), end_slope.y(), end_slope.z();
// create creator for known number of segments
piecewise_cubic_spline_creator_type pcsc( pts.size() - 1 );
// set the delta t for each curve segment
pcsc.set_t0( param[0] );
for ( size_t i = 0; i < ( param.size() - 1 ); ++i )
{
pcsc.set_segment_dt( param[i + 1] - param[i], i );
}
pcsc.set_clamped_cubic_spline( pts.begin(), sslope, eslope );
if ( !pcsc.create( m_Curve ) )
{
std::cerr << "Failed to create CSpline. " << __LINE__ << std::endl;
}
}