#include "StructureCurve.h"
#include "LineSegment.h"
using namespace Structure;
#include "qglviewer/quaternion.h"
#if defined(Q_OS_MAC)
#include <OpenGL/glu.h>
#else
#include <GL/glu.h>
#endif
#include <Eigen/Geometry>
Curve::Curve(const NURBS::NURBSCurved & newCurve, QString newID, QColor color)
{
this->curve = newCurve;
this->id = newID;
this->vis_property["color"] = color;
this->vis_property["showControl"] = false;
}
Node * Curve::clone()
{
if(!this) return NULL;
Curve * cloneCurve = new Curve( this->curve, this->id );
cloneCurve->property = this->property;
cloneCurve->vis_property = this->vis_property;
return cloneCurve;
}
QString Curve::type()
{
return CURVE;
}
Eigen::AlignedBox3d Curve::bbox(double scaling)
{
Eigen::AlignedBox3d box;
foreach(Vector3d cp, curve.getControlPoints())
box = box.merged( Eigen::AlignedBox3d(cp, cp) );
// Scaling
Eigen::Vector3d diagonal = box.diagonal() * 0.5;
Eigen::Vector3d a = box.center() + (diagonal * scaling);
Eigen::Vector3d b = box.center() - (diagonal * scaling);
box = box.merged( Eigen::AlignedBox3d(a,a) );
box = box.merged( Eigen::AlignedBox3d(b,b) );
return box;
}
std::vector<int> Curve::controlCount()
{
return std::vector<int>( 1, curve.GetNumCtrlPoints() );
}
std::vector<Vector3> Curve::controlPoints()
{
return curve.mCtrlPoint;
}
std::vector<Scalar> Curve::controlWeights()
{
return curve.mCtrlWeight;
}
void Curve::get( const Vector4d& coordinates, Vector3 & pos, std::vector<Vector3> & frame )
{
double u = coordinates[0];
Vector3 der1(0,0,0);
frame.resize(3, Vector3(0,0,0));
curve.GetFrame(u, pos, frame[0], frame[1], frame[2]);
}
SurfaceMesh::Vector3 Curve::position( const Vector4d& coordinates )
{
std::vector<Vector3d> nf = noFrame();
Vector3 p(0,0,0);
get(coordinates, p, nf);
return p;
}
Vector4d Curve::approxCoordinates( const Vector3 & pos )
{
Scalar t = curve.timeAt( pos );
return Vector4d( t, 0, 0, 0 );
}
SurfaceMesh::Vector3 Curve::approxProjection( const Vector3 & point )
{
Vector4d coords = approxCoordinates(point);
return curve.GetPosition(coords[0]);
}
Array2D_Vector3 Curve::discretized(Scalar resolution)
{
return curve.toSegments( resolution );
}
Array2D_Vector4d Curve::discretizedPoints( Scalar resolution )
{
Array2D_Vector4d result;
Scalar curveLength = curve.GetLength(0,1);
double firstTwoDist = (curve.mCtrlPoint[0] - curve.mCtrlPoint[1]).norm();
if(firstTwoDist < resolution * 0.001){
result.push_back( Array1D_Vector4d( 1, Vector4d(0,0,0,0) ) );
return result;
}
// For singular cases
if(curveLength < resolution){
result.push_back( Array1D_Vector4d( 1, Vector4d(0,0,0,0) ) );
return result;
}
if(!IsNumber(curveLength) || !IsFiniteNumber(curveLength)){
result.push_back( Array1D_Vector4d( 1, Vector4d(0,0,0,0) ) );
return result;
}
int np = 1 + (curveLength / resolution);
std::vector<Scalar> ptsTimes;
curve.SubdivideByLengthTime(np, ptsTimes);
Array1D_Vector4d resultTimes;
result.push_back( resultTimes );
foreach(Scalar t, ptsTimes)
result.back().push_back(Vector4d(t,0,0,0));
return result;
}
void Curve::laplacianSmoothControls( int num_iterations, std::set<int> anchored )
{
std::vector<Vector3> & cpnts = curve.mCtrlPoint;
// Special case anchoring
if(anchored.count(-1)){
anchored.clear();
anchored.insert(0);
anchored.insert(cpnts.size() - 1);
}
// Laplacian smoothing
for(int itr = 0; itr < num_iterations; itr++)
{
std::vector<Vector3> newCtrlPnts = cpnts;
for (int j = 0; j < (int)cpnts.size(); j++)
{
if(anchored.count(j) == 0)
newCtrlPnts[j] = (cpnts[j-1] + cpnts[j+1]) / 2.0;
}
for (int j = 0; j < (int)cpnts.size(); j++)
cpnts[j] = newCtrlPnts[j];
}
}
void Curve::moveBy( const Vector3d & delta )
{
curve.translate( delta );
}
std::vector<Vector3d> Curve::foldTo( Vector4d & foldPoint, bool isApply)
{
int cpIDX = controlPointIndexFromCoord(foldPoint);
Vector3 cp = curve.mCtrlPoint[cpIDX];
std::vector<Vector3d> deltas;
for(int i = 0; i < curve.mNumCtrlPoints; i++)
{
deltas.push_back(curve.mCtrlPoint[i] - cp);
if( isApply ) curve.mCtrlPoint[i] -= deltas.back();
}
return deltas;
}
void Curve::setControlPoints( const std::vector<Vector3> & newPositions )
{
assert(curve.mCtrlPoint.size() == newPositions.size());
curve.mCtrlPoint = newPositions;
}
void Curve::scale( Scalar scaleFactor )
{
this->curve.scale(scaleFactor);
}
void Curve::rotate( double angle, Vector3 axis )
{
angle *= 3.14159265358979 /180;
std::vector<Vector3> &mCtrlPoint = this->curve.mCtrlPoint;
for(int i = 0; i < (int)mCtrlPoint.size(); i++)
mCtrlPoint[i] = rotatedVec(mCtrlPoint[i], angle, axis);
}
Vector3 & Curve::controlPoint( int idx )
{
return curve.mCtrlPoint[idx];
}
int Curve::controlPointIndexFromCoord( Vector4d& coord )
{
return (curve.mCtrlPoint.size() - 1) * coord[0];
}
Vector3 & Curve::controlPointFromCoord( Vector4d& coord )
{
return curve.mCtrlPoint[controlPointIndexFromCoord( coord )];
}
SurfaceMesh::Scalar Curve::area()
{
return curve.GetTotalLength();
}
SurfaceMesh::Vector3 Curve::center()
{
Vector3 pos(0,0,0);
std::vector<Vector3d> nf = noFrame();
get(Vector4d(0.5,0.5,0,0), pos, nf);
return pos;
}
void Curve::draw(bool isShowCtrlPts)
{
if(vis_property["glow"].toBool())
{
NURBS::CurveDraw::draw( &curve, Qt::yellow, isShowCtrlPts, 2.5 );
}
else
{
NURBS::CurveDraw::draw( &curve, vis_property["color"].value<QColor>(), isShowCtrlPts );
}
glPointSize(10);
glColor3d(1,0,1);
glBegin(GL_POINTS);
foreach(Vector3 p, curve.misc_points) glVector3(p);
glEnd();
// Draw selections
GLUquadricObj *quadObj = gluNewQuadric();
gluQuadricDrawStyle(quadObj, GLU_FILL);
gluQuadricNormals(quadObj, GLU_SMOOTH);
foreach (int pID, selections.keys())
{
QColor color = selections[pID];
glColor3d(color.red(), color.green(), color.blue());
Vector3 p = curve.GetControlPoint(pID);
glPushMatrix();
glTranslatef(p.x(), p.y(), p.z());
gluSphere(quadObj, 0.02, 16, 16);
glPopMatrix();
}
gluDeleteQuadric(quadObj);
}
void Curve::drawWithNames( int nID, int pointIDRange )
{
int pID = nID * pointIDRange;
glPointSize(20.0f);
for(int i = 0; i < curve.mNumCtrlPoints; i++)
{
glPushName(pID++);
Vector3d p = curve.GetControlPoint(i);
glBegin(GL_POINTS);
glVertex3d(p.x(), p.y(), p.z());
glEnd();
glPopName();
}
}
void Curve::equalizeControlPoints( Node * _other )
{
Curve *other = (Curve *)_other;
int targetNumber = other->curve.mNumCtrlPoints;
int diff = targetNumber - curve.mNumCtrlPoints;
if(diff == 0.0) return;
std::vector<Vector3d> newPnts = this->curve.simpleRefine( diff );
this->curve = NURBS::NURBSCurved(newPnts, std::vector<double>( newPnts.size(), 1.0 ));
}
int Curve::numCtrlPnts()
{
return curve.mNumCtrlPoints;
}
void Curve::refineControlPoints( int nU, int nV /*= 0*/ )
{
nV = nV;
int diff = nU - curve.mNumCtrlPoints;
if(diff == 0) return;
std::vector<Vector3d> newPnts = this->curve.simpleRefine( diff );
this->curve = NURBS::NURBSCurved(newPnts, std::vector<double>( newPnts.size(), 1.0 ));
}
Vector3d Curve::direction()
{
Vector3d dir = curve.mCtrlPoint.back() - curve.mCtrlPoint.front();
return dir.normalized();
}
/* Curve encoding, to decode you need two points A,B and a frame XYZ */
CurveEncoding Curve::encodeCurve( Array1D_Vector3 points, Vector3 start, Vector3 end, bool isFlip )
{
CurveEncoding cpCoords;
NURBS::Line segment(start, end);
// compute frame
qglviewer::Vec x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
Vector3d Z = (end - start).normalized();
qglviewer::Quaternion q(z, qglviewer::Vec(Z));
x = q * x; y = q * y;
Vector3d X(x[0], x[1], x[2]);
Vector3d Y(y[0], y[1], y[2]);
for(int i = 0; i < (int)points.size(); i++)
{
double t;
Vector3 p = points[i], proj;
segment.ClosestPoint(p, t, proj);
Vector3 dir = p - proj;
// Parameters: t, offset, theta, psi
Array1D_Real params(4,0);
params[0] = t;
if(dir.norm() > 0){
params[1] = dir.norm() / segment.length;
dir = dir.normalized();
}
globalToLocalSpherical(X,Y,Z, params[2], params[3], dir);
// Flipping case
int idx = i;
if(isFlip) idx = (points.size()-1) - i;
cpCoords[idx] = params;
}
return cpCoords;
}
CurveEncoding Curve::encodeCurve( Curve * curve, Vector3 start, Vector3 end, bool isFlip )
{
return encodeCurve(curve->controlPoints(),start,end,isFlip);
}
Array1D_Vector3 Curve::decodeCurve(CurveEncoding cpCoords, Vector3 start, Vector3 end, double T)
{
Array1D_Vector3 controlPoints (cpCoords.size(), Vector3(0,0,0));
NURBS::Line segment(start, end);
if(segment.length < DECODE_ZERO_THRESHOLD){
return Array1D_Vector3( cpCoords.size(), start );
}
// compute frame
qglviewer::Vec x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
Vector3d Z = (end - start).normalized();
qglviewer::Quaternion q(z, qglviewer::Vec(Z));
x = q * x; y = q * y;
Vector3d X(x[0], x[1], x[2]);
Vector3d Y(y[0], y[1], y[2]);
for(int i = 0; i < (int)controlPoints.size(); i++)
{
double t = cpCoords[i][0];
double offset = cpCoords[i][1];
double theta = cpCoords[i][2];
double psi = cpCoords[i][3];
Vector3 dir(0,0,0);
localSphericalToGlobal(X,Y,Z,theta,psi,dir);
controlPoints[i] = segment.pointAt(t) + (dir * ((offset * T) * segment.length));
}
return controlPoints;
}
void Curve::deformTo( const Vector4d & handle, const Vector3 & to, bool isRigid )
{
Vector4d otherEndCoord = Vector4d((handle[0] > 0.5) ? 0 : 1);
Vector3 p = position( handle );
double diff = (p-to).norm();
if(diff < DECODE_ZERO_THRESHOLD) return;
if( isRigid )
{
Vector3d delta = to - p;
this->moveBy( delta );
return;
}
Eigen::Vector3d otherEnd = position( otherEndCoord );
// Find new scale
double d = (p - otherEnd).norm();
if(d < DECODE_ZERO_THRESHOLD) return;
double scale = (to - otherEnd).norm() / d;
// Find minimum rotation
Eigen::Vector3d dirFrom (p - otherEnd);
Eigen::Vector3d dirTo (to - otherEnd);
dirFrom.normalize();
dirTo.normalize();
Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(dirFrom, dirTo).normalized();
Array1D_Vector3 ctrlPnts = controlPoints();
// Compute deltas and apply transformations
for(int i = 0; i < (int)ctrlPnts.size(); i++)
{
Eigen::Vector3d delta = (ctrlPnts[i] - otherEnd);
double length = delta.norm() * scale;
delta.normalize();
Eigen::Vector3d rotated = rotation * delta;
ctrlPnts[i] = Vector3( (rotated * length) + otherEnd );
}
setControlPoints( ctrlPnts );
}
void Curve::deformTwoHandles( Vector4d& handleA, Vector3 newPosA, Vector4d& handleB, Vector3 newPosB )
{
Vector3d oldA = position(handleA);
Vector3d oldB = position(handleB);
// Numerical checks
double oldDiff = (oldA-oldB).norm(); if(oldDiff < DECODE_ZERO_THRESHOLD) return;
double diff = (newPosA-newPosB).norm(); if(diff < DECODE_ZERO_THRESHOLD) return;
setControlPoints( decodeCurve( encodeCurve(this, oldA, oldB), newPosA, newPosB ) );
}