/*
 * NodeNurbsCurve.cpp
 *
 * Copyright (C) 2003 Th. Rothermel
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program (see the file "COPYING" for details); if 
 * not, write to the Free Software Foundation, Inc., 675 Mass Ave, 
 * Cambridge, MA 02139, USA.
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#ifndef _WIN32
# include "stdlib.h"
#endif

#include "stdafx.h"
#include "NodeNurbsCurve.h"
#include "Scene.h"
#include "FieldValue.h"
#include "SFInt32.h"
#include "MFFloat.h"
#include "MFInt32.h"
#include "MFVec2f.h"
#include "MFVec3f.h"
#include "SFNode.h"
#include "SFBool.h"
#include "SFVec3f.h"
#include "Vec2f.h"
#include "Vec3f.h"
#include "RenderState.h"
#include "DuneApp.h"
#include "NodeCoordinate.h"
#include "NodeNormal.h"
#include "NodeTextureCoordinate.h"
#include "NodeNurbsSurface.h"
#include "NurbsMakeRevolvedSurface.h"
#include "NurbsCurveDegreeElevate.h"
#include "Util.h"
#include "NodeIndexedLineSet.h"
#include "NodePositionInterpolator.h"
#include "NodeOrientationInterpolator.h"
#include "NodeSuperExtrusion.h"

ProtoNurbsCurve::ProtoNurbsCurve(Scene *scene)
  : Proto(scene, "NurbsCurve")
{

    controlPoint.set(
          addExposedField(MFVEC3F, "controlPoint", new MFVec3f()));
    tessellation.set(
          addExposedField(SFINT32, "tessellation", new SFInt32(0)));
    weight.set(
          addExposedField(MFDOUBLE, "weight", new MFDouble(), new SFFloat(0.0f)));
    knot.set(
          addField(MFDOUBLE, "knot", new MFDouble()));
    order.set(
          addField(SFINT32, "order", new SFInt32(3), new SFInt32(2)));
}

Node *
ProtoNurbsCurve::create(Scene *scene)
{
    return new NodeNurbsCurve(scene, this); 
}

NodeNurbsCurve::NodeNurbsCurve(Scene *scene, Proto *proto)
  : Node(scene, proto)
{
    _chain.resize(0);
    _chainDirty = true;
}

void
NodeNurbsCurve::draw()
{
  if(_chainDirty) {
    createChain();
    _chainDirty = false;
  }
  
  if (_chain.size() == 0) 
      createChain();

  int i;
  
  glPushAttrib(GL_ENABLE_BIT);
  glDisable(GL_LIGHTING);
  glDisable(GL_TEXTURE_2D);
  glEnable(GL_LINE_SMOOTH);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glEnable(GL_BLEND);
  glLineWidth(1);
  
  //taken from IndexedLineSet if colors == NULL

  float	    c[4];
  glGetMaterialfv(GL_FRONT, GL_EMISSION, c);
  Util::myGlColor4fv(c);

  glBegin(GL_LINE_STRIP);
  for(i=0; i<_chain.size(); i++){
    glVertex3f(_chain[i].x, _chain[i].y, _chain[i].z);
  }
  glEnd();
  glEnable(GL_LIGHTING);
  glPopAttrib();


}

void
NodeNurbsCurve::createChain()
{
  int iTess = tessellation()->getValue();
  float *weights = NULL;
  int iDimension = controlPoint()->getSize() / 3;
  int i;

  if(weight()->getSize() == 0) {
    weights = new float[iDimension];
    for(i=0; i<iDimension; i++){
      weights[i] = 1.0f;
    }
  } 
  else if(weight()->getSize() != iDimension) {
    return;
  }
  
  if(iTess<=0){
    iTess = 32;
  }
  
  int size = iTess + 1;
  Vec3f *tess = new Vec3f[size];
  
  const float *knots = knot()->getValues();
  
  float inc = (knots[knot()->getSize()-1] - knots[0]) / iTess;
  
  //  int index = 0;
  float u;
  
  const float *w = weights ? weights : weight()->getValues();
  
  for (i=0, u=knots[0]; i<=iTess; i++, u = u + inc){
    tess[i] = curvePoint(iDimension, order()->getValue(), knots, 
			 (const Vec3f *) controlPoint()->getValues(),
			 w, u);
  }

  _chain.resize(size);  
  for(i=0; i<size; i++){
    _chain[i] = tess[i];
  }
}  
  

void
NodeNurbsCurve::drawHandles()
{
    int		iDimension = controlPoint()->getSize() / 3;
    RenderState	state;

    if (weight()->getSize() != iDimension) {
	return;
    }

    glPushName(iDimension + 1);
    glDisable(GL_LIGHTING);
    Util::myGlColor3f(1.0f, 1.0f, 1.0f);
    glBegin(GL_LINE_STRIP);
    for (int i = 0; i < iDimension; i++) {
        //glBegin(GL_LINE_STRIP);
	const float *v = controlPoint()->getValue(i);
	float	w = weight()->getValue(i);
	glVertex3f(v[0] / w, v[1] / w, v[2] / w);
	//glEnd();
    }
    glEnd();
    
    int ci;

    state.startDrawHandles();
    //glLoadName(ci);
    for (ci = 0; ci < iDimension; ci++) {
        glLoadName(ci);
	state.drawHandle(
		Vec3f(controlPoint()->getValue(ci)) / weight()->getValue(ci));
    }
    state.endDrawHandles();
    glPopName();
    glEnable(GL_LIGHTING);
}


Vec3f
NodeNurbsCurve::getHandle(int handle, int *constraint,
			    int *field)
{
    *constraint = CONSTRAIN_NONE;
    *field = controlPoint_Index() ;

    if (handle >= 0 && handle < controlPoint()->getSize() / 3) {
	Vec3f ret((Vec3f)controlPoint()->getValue(handle) / 
                   weight()->getValue(handle));
        return ret;
    } else {
        return Vec3f(0.0f, 0.0f, 0.0f);
    }
}


void
NodeNurbsCurve::setHandle(int handle, const Vec3f &v) 
{
    MFVec3f    *oldValue = controlPoint();
    MFVec3f    *newValue = (MFVec3f *) oldValue->copy();

    if (handle >= 0 && handle < oldValue->getSize() / 3) {
        Vec3f	v2 = v * weight()->getValue(handle);	 
	_chainDirty = true;
	newValue->setValue(handle * 3, v2.x);
	newValue->setValue(handle * 3+1, v2.y);
	newValue->setValue(handle * 3+2, v2.z);
	_scene->setField(this, controlPoint_Index(), newValue);
    }
}

void
NodeNurbsCurve::setField(int index, FieldValue *value) 
{
    _chainDirty = true;
    Node::setField(index, value);
}

int
NodeNurbsCurve::findSpan(int dimension, int order, float u, 
			   const float knots[])
{
    int		low, mid, high;
    int		n = dimension + order - 1;

    if (u >= knots[n]) {
	return n - order;
    }
    low = order - 1;	high = n - order + 1;

    mid = (low + high) / 2;

    int oldLow = low;
    int oldHigh = high;
    int oldMid = mid;
    while (u < knots[mid] || u >= knots[mid+1]) {
	if (u < knots[mid]) {
	    high = mid;
	} else {
	    low = mid;
	}
	mid = (low+high)/2;
        // emergency abort of loop, otherwise a endless loop can occure
        if ((low == oldLow) && (high == oldHigh) && (mid == oldMid))
            break;

        oldLow = low;
        oldHigh = high;
        oldMid = mid;
    }
    return mid;
}

void
NodeNurbsCurve::basisFuns(int span, float u, int order,     
			    const float knots[], float basis[],
			    float deriv[])
{
    float      *left = (float *) malloc(order * sizeof(float));
    float      *right = (float *) malloc(order * sizeof(float));

    if ((left==NULL) || (right==NULL))
       return;
    basis[0] = 1.0f;
    for (int j = 1; j < order; j++) {
	left[j] = u - knots[span+1-j];
	right[j] = knots[span+j]-u;
	float saved = 0.0f, dsaved = 0.0f;
	for (int r = 0; r < j; r++) {
	    float temp = basis[r] / (right[r+1] + left[j-r]);
	    basis[r] = saved + right[r+1] * temp;
	    deriv[r] = dsaved - j * temp;
	    saved = left[j-r] * temp;
	    dsaved = j * temp;
	}
	basis[j] = saved;
	deriv[j] = dsaved;
    }
    free(left);
    free(right);
}

Vec3f
NodeNurbsCurve::curvePoint(int dimension, int order, 
			       const float knots[],
			       const Vec3f controlPoints[],
			       const float weight[], float u)
{
    int i; 
    float      *basis = (float *) malloc(order * sizeof(float));
    float      *deriv = (float *) malloc(order * sizeof(float));

    if((basis==NULL) || (deriv==NULL)){
      return NULL;
    }
    
    int	span = findSpan(dimension, order, u, knots);

    basisFuns(span, u, order, knots, basis, deriv);

    Vec3f C(0.0f, 0.0f, 0.0f);
    float w = 0.0f;
    for(i=0; i<order; i++){
      C += controlPoints[span-order+1+i] * basis[i];
      w += weight[span-order+1+i] * basis[i];
    }   
    
    C = C / w;

    free(basis);
    free(deriv);
    
    return C;
}


bool
NodeNurbsCurve::writeEXTERNPROTO(int f)
{
    RET_ONERROR( mywritestr(f ,"EXTERNPROTO NurbsCurve[\n") )    
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," exposedField MFVec3f controlPoint\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," exposedField SFInt32 tessellation\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," exposedField MFFloat weight\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," field MFFloat knot\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," field SFInt32 order\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," ]\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ,"[\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," \"urn:web3d:vrml97:node:NurbsCurve\",\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," \"urn:inet:blaxxun.com:node:NurbsCurve\",\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ," \"urn:ParaGraph:NurbsCURVE\",\n") )
    TheApp->incSelectionLinenumber();
#ifdef HAVE_VRML97_AMENDMENT1_PROTO_URL
    RET_ONERROR( mywritestr(f ," \"") )
    RET_ONERROR( mywritestr(f ,HAVE_VRML97_AMENDMENT1_PROTO_URL) )
    RET_ONERROR( mywritestr(f ,"/NurbsCurvePROTO.wrl") )
    RET_ONERROR( mywritestr(f ,"\"\n") )
    TheApp->incSelectionLinenumber();
#else
    RET_ONERROR( mywritestr(f ," \"NurbsCurvePROTO.wrl\",\n") )
    TheApp->incSelectionLinenumber();
#endif
    RET_ONERROR( mywritestr(f ," \"http://www.csv.ica.uni-stuttgart.de/vrml/dune/docs/vrml97Amendment1/NurbsCurvePROTO.wrl\"\n") )
    TheApp->incSelectionLinenumber();
    RET_ONERROR( mywritestr(f ,"]\n") )
    TheApp->incSelectionLinenumber();
    return true;
}

int             
NodeNurbsCurve::write(int filedes, int indent)
{
    if (_scene->isPureVRML97()) {
        Node * node = toIndexedLineSet();
        RET_ONERROR( node->write(filedes, indent) )
        node->unref();
    } else
        RET_ONERROR( NodeData::write(filedes, indent) )
    return 0;
}

Node * 
NodeNurbsCurve::toIndexedLineSet(void)
{
    int i;

    if(_chainDirty) {
       createChain();
       _chainDirty = false;
    }
  
    if (_chain.size() == 0) 
        createChain();

    NodeCoordinate *ncoord = (NodeCoordinate *)_scene->createNode("Coordinate");

    int chainLength = _chain.size();
    float *chainValues = new float[chainLength * 3];
    for (i = 0; i < chainLength; i++) {
        chainValues[i * 3    ] = _chain[i].x;
        chainValues[i * 3 + 1] = _chain[i].y;
        chainValues[i * 3 + 2] = _chain[i].z;
    }
    ncoord->point(new MFVec3f(chainValues, chainLength * 3));
    NodeIndexedLineSet *node = (NodeIndexedLineSet *)
                               _scene->createNode("IndexedLineSet");
    node->coord(new SFNode(ncoord));
    
    int *chainIndex = new int[chainLength];
    for (i = 0; i < chainLength; i++)
        chainIndex[i] = i;
    node->coordIndex(new MFInt32(chainIndex, chainLength));
    _chain.resize(0);
    _chainDirty = true;
    return node;
}

Node*
NodeNurbsCurve::toNurbs(int narcs, int pDegree, float rDegree, Vec3f &P1, Vec3f &P2)
{
  NodeNurbsSurface *node = (NodeNurbsSurface *)
                            _scene->createNode("NurbsSurface");
  
  int i;
  Vec3f *tmpControlPoints = new Vec3f[controlPoint()->getSFSize()];
  float *tmpWeights = new float[weight()->getSize()];
  float *vKnots = new float [knot()->getSize()];
  int vOrder = order()->getValue();
  int vDimension = weight()->getSize();
  
  
  for(i=0; i<(controlPoint()->getSFSize()); i++){
    tmpControlPoints[i] = controlPoint()->getValue(i);
  }

  for(i=0; i<(weight()->getSFSize()); i++){
    tmpWeights[i] = weight()->getValue(i);
  }
  
  for(i=0; i<(knot()->getSFSize()); i++){
    vKnots[i] = knot()->getValue(i);
  }
    
  Vec3f point = P1;
  Vec3f vector = P2 - P1;
  vector.normalize();
  if((vector.x==0) && (vector.y==0) && (vector.z==0)) {return NULL;}
  
  NurbsMakeRevolvedSurface surface(tmpControlPoints, tmpWeights, vDimension, narcs, rDegree, pDegree, point, vector);
  if (!surface.isValid())
      return NULL;

  int uOrder = pDegree + 1;
  float *controlPoints = new float[surface.getPointSize()];
  float *weights = new float[surface.getWeightSize()];
  int uDimension = surface.getWeightSize() / vDimension;
  float *uKnots = new float[uDimension + uOrder]; 

  //get results of rotation
  //get control points
  for(i=0; i<(surface.getPointSize()); i++){
    controlPoints[i] = surface.getControlPoints(i);
  }
  //weights
  for(i=0; i<(surface.getWeightSize()); i++){
    weights[i] = surface.getWeights(i);
  }
  if (pDegree == 1){
    //set u-knotvektor
    for(i=0; i<uOrder; i++){
      uKnots[i] = 0.0f;
      uKnots[i+uDimension] = (float) (uDimension - uOrder +1);
    }
    for(i=0; i<(uDimension-uOrder); i++){
      uKnots[i+uOrder] = (float) (i + 1);  
    } 
  }  
  else {
    //u-knotenvector
    for(i=0; i<(surface.getKnotSize()); i++){
      uKnots[i] = surface.getKnots(i);
    }
  }
  
  node->setField(node->uDimension_Index(), new SFInt32(uDimension));
  node->setField(node->vDimension_Index(), new SFInt32(vDimension));
  node->uKnot(new MFFloat(uKnots, uDimension + uOrder));
  node->vKnot(new MFFloat(vKnots, vDimension + vOrder));
  node->setField(node->uOrder_Index(), new SFInt32(uOrder));
  node->setField(node->vOrder_Index(), new SFInt32(vOrder));
  node->controlPoint(new MFVec3f(controlPoints, uDimension * vDimension * 3));
  node->weight(new MFFloat(weights, uDimension * vDimension));

  return node;
}  

Node*
NodeNurbsCurve::toSuperExtrusion(void)
{
  NodeSuperExtrusion *node = (NodeSuperExtrusion *)
                            _scene->createNode("SuperExtrusion");
  
  int i;
  float *tmpControlPoints = new float[controlPoint()->getSize()];
  float *tmpWeights = new float[weight()->getSize()];
  float *tmpKnots = new float[knot()->getSize()];
  int tmpOrder = order()->getValue();  
  
  for(i=0; i<(controlPoint()->getSize()); i++){
    tmpControlPoints[i] = controlPoint()->getValues()[i];
  }

  for(i=0; i<(weight()->getSFSize()); i++){
    tmpWeights[i] = weight()->getValue(i);
  }
  
  for(i=0; i<(knot()->getSFSize()); i++){
    tmpKnots[i] = knot()->getValue(i);
  }
    
  node->setField(node->spineTessellation_Index(), 
                 new SFInt32(tessellation()->getValue()));
  node->knot(new MFFloat(tmpKnots, knot()->getSFSize()));
  node->setField(node->order_Index(), new SFInt32(tmpOrder));
  node->controlPoint(new MFVec3f(tmpControlPoints, controlPoint()->getSize()));
  node->weight(new MFFloat(tmpWeights, weight()->getSFSize()));

  return node;
}  

void 
NodeNurbsCurve::flatter(int change, int zero)
{
    MFVec3f    *values = controlPoint();
    for (int i = 0; i < values->getSFSize(); i++) {
        SFVec3f vec(values->getValue(i));
        float len = sqrt(vec.getValue(change) * vec.getValue(change)  
                       + vec.getValue(zero)   * vec.getValue(zero));
        vec.setValue(change,  len);
        vec.setValue(zero, 0);
        SFVec3f result(vec.getValue(0), vec.getValue(1), vec.getValue(2));
        values->setSFValue(i, &result);
    } 
}

bool
NodeNurbsCurve::flatten(int direction)
{
    switch(direction) {
      case NURBS_ROT_X_AXIS:
        flatter(2, 1);
        break;
      case NURBS_ROT_Y_AXIS:
        flatter(0, 2);
        break;
      case NURBS_ROT_Z_AXIS:
        flatter(1, 0);
        break;
      case NURBS_ROT_POINT_TO_POINT:
        return false; 
      default:
        assert(1);
    }
    return true;
}

Vec3f               
NodeNurbsCurve::getMinBoundingBox(void)
{
    if (_chainDirty) {
        createChain();
        _chainDirty = false;
    }
  
    if (_chain.size() == 0) 
        createChain();

    int chainLength = _chain.size();
    float *chainValues = new float[chainLength * 3];
    for (int i = 0; i < chainLength; i++) {
        chainValues[i * 3    ] = _chain[i].x;
        chainValues[i * 3 + 1] = _chain[i].y;
        chainValues[i * 3 + 2] = _chain[i].z;
    }
    MFVec3f chain(chainValues, chainLength * 3);
    return chain.getMinBoundingBox();
}

Vec3f               
NodeNurbsCurve::getMaxBoundingBox(void)
{
    if (_chainDirty) {
        createChain();
        _chainDirty = false;
    }
  
    if (_chain.size() == 0) 
        createChain();

    int chainLength = _chain.size();
    float *chainValues = new float[chainLength * 3];
    for (int i = 0; i < chainLength; i++) {
        chainValues[i * 3    ] = _chain[i].x;
        chainValues[i * 3 + 1] = _chain[i].y;
        chainValues[i * 3 + 2] = _chain[i].z;
    }
    MFVec3f chain(chainValues, chainLength * 3);
    return chain.getMaxBoundingBox();
}

void
NodeNurbsCurve::flip(int index)
{
    if (controlPoint())
        controlPoint()->flip(index);
    _chainDirty = true;
    
}

Node*
NodeNurbsCurve::degreeElevate(int newDegree)
{
  //Nothing but simple application of existing class "NurbsCurveDegreeElevate" 

  if(newDegree <= ((order()->getValue())-1)){
    return NULL;
  }

  NodeNurbsCurve *node = (NodeNurbsCurve *)
                            _scene->createNode("NurbsCurve");

  if(newDegree > ((order()->getValue())-1)){
    
    //load old values
    int i;
    int dimension = controlPoint()->getSize() / 3;
    Vec3f *tPoints = new Vec3f[dimension];
    float *tWeights = new float[dimension];
    int tOrder = order()->getValue();
    int knotSize = knot()->getSize();
    Array<float> tKnots(knotSize);
    int deg = order()->getValue() - 1;
    int upDegree = newDegree - deg;

    for (i=0; i<dimension; i++){
      tPoints[i] = controlPoint()->getValue(i);
      tWeights[i] =weight()->getValue(i);
    }
    
    for (i=0; i<knotSize; i++){
      tKnots[i] = knot()->getValue(i);
    }

    //elevate curve
    NurbsCurveDegreeElevate elevatedCurve(tPoints, tWeights, tKnots, dimension, deg, upDegree);

    //get new values
    int newDimension = elevatedCurve.getPointSize();
    int newKnotSize = elevatedCurve.getKnotSize();
    int newOrder = newDegree + 1;

    float *newControlPoints = new float[newDimension * 3];
    float *newWeights = new float[newDimension];
    float *newKnots = new float[newDimension + newOrder]; 

    for (i=0; i<newDimension; i++){
      newControlPoints[(i*3)]   = elevatedCurve.getControlPoints(i).x;
      newControlPoints[(i*3)+1] = elevatedCurve.getControlPoints(i).y;
      newControlPoints[(i*3)+2] = elevatedCurve.getControlPoints(i).z;
      newWeights[i] = elevatedCurve.getWeights(i);
    }
    
    for (i=0; i<newKnotSize; i++){
      newKnots[i] = elevatedCurve.getKnots(i);
    }

    //load new node
    node->controlPoint(new MFVec3f(newControlPoints, newDimension * 3));    
    node->weight(new MFFloat(newWeights, newDimension));
    node->knot(new MFFloat(newKnots, newDimension + newOrder));
    node->order(new SFInt32(newOrder));
    node->tessellation(new SFInt32(tessellation()->getValue()));
 
    return node;
  }
  return NULL;
}

Node * 
NodeNurbsCurve::toPositionInterpolator(void)
{
    int i;

    if(_chainDirty) {
       createChain();
       _chainDirty = false;
    }
  
    if (_chain.size() == 0) 
        createChain();

    NodePositionInterpolator *npos = (NodePositionInterpolator *)
                                     _scene->createNode("PositionInterpolator");

    int chainLength = _chain.size();
    float *chainValues = new float[chainLength * 3];
    for (i = 0; i < chainLength; i++) {
        chainValues[i * 3    ] = _chain[i].x;
        chainValues[i * 3 + 1] = _chain[i].y;
        chainValues[i * 3 + 2] = _chain[i].z;
    }
    npos->keyValue(new MFVec3f(chainValues, chainLength * 3));
    float *chainKeyes = new float[chainLength];
    for (i = 0; i < chainLength; i++)
        chainKeyes[i] = 1.0 / (chainLength - 1) * i;
    npos->key(new MFFloat(chainKeyes, chainLength));
    _chain.resize(0);
    _chainDirty = true;
    return npos;
}

Node * 
NodeNurbsCurve::toOrientationInterpolator(void)
{
    int i;

    if(_chainDirty) {
       createChain();
       _chainDirty = false;
    }
  
    if (_chain.size() == 0) 
        createChain();

    NodeOrientationInterpolator *npos = (NodeOrientationInterpolator *)
                                 _scene->createNode("OrientationInterpolator");

    int chainLength = _chain.size();
    if (chainLength < 3)
        return npos;
    float *chainValues = new float[(chainLength - 1) * 4];
    Quaternion oldQuat(0, 0 , 0, 1);
    for (i = 1; i < (chainLength - 1); i++) {
        Vec3f point1(_chain[i - 1].x,  _chain[i - 1].y, _chain[i - 1].z);
        Vec3f point2(_chain[i    ].x,  _chain[i    ].y, _chain[i    ].z);
        Vec3f point3(_chain[i + 1].x,  _chain[i + 1].y, _chain[i + 1].z);
        Vec3f vec1(point2 - point1);
        Vec3f vec2(point3 - point2);
        Vec3f rotationAxis = vec1.cross(vec2);
        float sinangle = rotationAxis.length() / vec1.length() / vec2.length();
        float cosangle = vec1.dot(vec2) / vec1.length() / vec2.length();
        float angle = 0;
        if (sinangle != 0)
            angle = atan2(sinangle, cosangle);
        Vec3f axis(-rotationAxis.x, -rotationAxis.y, -rotationAxis.z);
//        rotationAxis.normalize();
        axis.normalize();
        Quaternion quat(axis, -angle);
        quat.normalize();
        quat = quat * oldQuat;
        SFRotation rot(quat);
        chainValues[i * 4    ] =  rot.getValue()[0];
        chainValues[i * 4 + 1] =  rot.getValue()[1];
        chainValues[i * 4 + 2] =  rot.getValue()[2];
        chainValues[i * 4 + 3] =  rot.getValue()[3];
        oldQuat = quat;
    }

    chainValues[0] = 0;
    chainValues[1] = 0;
    chainValues[2] = 1;
    chainValues[3] = 0;
    npos->keyValue(new MFRotation(chainValues, (chainLength - 1) * 4));

    float *chainKeyes = new float[chainLength - 1];
    for (i = 0; i < (chainLength - 1); i++)
        chainKeyes[i] = 1.0 / (chainLength - 2) * i;
    npos->key(new MFFloat(chainKeyes, chainLength - 1));
    _chain.resize(0);
    _chainDirty = true;
    return npos;
}

const Vec3f *
NodeNurbsCurve::getChain()
{
    if(_chainDirty) {
       createChain();
       _chainDirty = false;
    }
    return _chain.getData();
}

int
NodeNurbsCurve::getChainLength()
{
    if(_chainDirty) {
       createChain();
       _chainDirty = false;
    }
    return _chain.size();
}