Node.cpp

#include <math.h>

#ifdef WIN32
#include <windows.h>
#endif

#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <GL/glui.h>

// #include "LinearR3.h"
#include "MathMisc.h"
#include "Node.h"
#include <kdl/frames_io.hpp>
extern int RotAxesOn;

Node::Node(const KDL::Vector& attach, const KDL::Vector& v, double size, Purpose purpose, double minTheta, double maxTheta, double restAngle)
{
	Node::freezed = false;
	Node::size = size;
	Node::purpose = purpose;
	seqNumJoint = -1;
	seqNumEffector = -1;
	Node::attach = attach;		// Global attachment point when joints are at zero angle
	SetToZero(r);		// r will be updated when this node is inserted into tree
	Node::v = v;				// Rotation axis when joints at zero angles
	theta = 0.0;
	Node::minTheta = minTheta;
	Node::maxTheta = maxTheta;
	Node::restAngle = restAngle;
	left = right = realparent = 0;
}

// Compute the global position of a single node
void Node::ComputeS(void)
{
	Node* y = this->realparent;
	Node* w = this;
	s = r;							// Initialize to local (relative) position
	while ( y ) {
                s=KDL::Rotation::Rot2(y->v, y->theta)*s;
                //s.Rotate( y->theta, y->v );
		y = y->realparent;
		w = w->realparent;
		s += w->r;
	}
}

// Compute the global rotation axis of a single node
void Node::ComputeW(void)
{
	Node* y = this->realparent;
	w = v;							// Initialize to local rotation axis
	while (y) {
                w=KDL::Rotation::Rot2(y->v, y->theta)*w;
		//w.Rotate(y->theta, y->v);
		y = y->realparent;
	}
}

// Draw the box from the origin to point r.
void Node::DrawBox() const
{

	glPushMatrix();

	/*	if (r.getx()) {
		beta = atan2(r.gety(), sqrt(r.getx()*r.getx()+r.getz()*r.getz()));
		glRotatef(beta*180./M_PI, -r.getz(), 0.0f, r.getx());
	} else if (r.getz()) {
		beta = atan2(r.gety(), r.getz());
		glRotatef(beta*180./M_PI, -1.0f, 0.0f, 0.0f);			
	} else {
		if (r.gety() >= 0) {
			glRotatef(90., -1.0f, 0.0f, 0.0f);
		} else {
			glRotatef(90., 1.0f, 0.0f, 0.0f);
		}
	} */
	
	if ( r.z()!=0.0 || r.x()!=0.0 ) {
		double alpha = atan2(r.z(), r.x());
		glRotatef(alpha*RadiansToDegrees, 0.0f, -1.0f, 0.0f);
	}

	if ( r.y()!=0.0 ) {
		double beta = atan2(r.y(), sqrt(r.x()*r.x()+r.z()*r.z()));
		glRotatef( beta*RadiansToDegrees, 0.0f, 0.0f, 1.0f );
	}

	double length = r.Norm();
	glScalef(length/size, 1.0f, 1.0f);
	glTranslatef(size/2, 0.0f, 0.0f);

	glutSolidCube(size);

	glPopMatrix();
}

void Node::DrawNode(bool isRoot)
{
	if (!isRoot) {
		DrawBox();
	}

	if (RotAxesOn) {
		const double rotAxisLen = 1.3;
		glDisable(GL_LIGHTING);
		glColor3f(1.0f, 1.0f, 0.0f);
		glLineWidth(2.0);
		glBegin(GL_LINES);
		KDL::Vector temp = r;
                temp+=v*(rotAxisLen*size);
		//temp.AddScaled(v,rotAxisLen*size);
		glVertex3f( temp.x(), temp.y(), temp.z() );
                temp+=v*(-2.0*rotAxisLen*size);
                //temp.AddScaled(v,-2.0*rotAxisLen*size);
		glVertex3f( temp.x(), temp.y(), temp.z() );
		glEnd();
		glLineWidth(1.0);
		glEnable(GL_LIGHTING);
	}
	glTranslatef(r.x(), r.y(), r.z());
	glRotatef(theta*RadiansToDegrees, v.x(), v.y(), v.z());
}

using namespace std;

void Node::PrintNode()
{
	cerr << "Attach : (" << attach << ")\n";
	cerr << "r : (" << r << ")\n";
	cerr << "s : (" << s << ")\n";
	cerr << "w : (" << w << ")\n";
	cerr << "realparent : " << realparent->seqNumJoint << "\n";
}

void Node::InitNode()
{
	theta = 0.0;
}