# Rotate object to face towards point

This topic is 2875 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

## Recommended Posts

Hello all, So, the problem I'm trying to solve is this: I have a fish swimming towards an object representing some bait. I have no problem moving the fish towards the bait, however I also need to orient the fish in the direction it is swimming, and I can't figure out how to do that correctly. Here is what I'm doing now: VecDisplacement (VecNewPos - VecCurrentPos) VecHeading (starts out as (0, 1, 0), as the fish is facing that direction when loaded) So I find the axis to rotate about: VecAxis = VecHeading x VecDisplacement Then finding the angle to rotate: |VecHeading x VecDisplacement| = |VecHeading|*|VecDisplacement|*sin(a) Then updating the heading vector: VecHeading = VecDisplacement And then passing that to glRotatef: glRotatef(turnAngle, turnAxis->x, turnAxis->y, turnAxis->x); This doesn't quit work, so what am I doing wrong / missing? Note that I'm not trying to get motion rotation, but if someone could explain how to do that too, that would be great (not the primary thing though).

##### Share on other sites
There's not quite enough information there to tell you why your current code isn't working, mostly because we don't know in what context glRotatef() is being used. (That is, what's the state of the matrix stack when it's invoked? Are you saving the orientation somehow after it's been modified, e.g. using glGetFloatv()? And so on.)

In general though, you wouldn't use glRotatef() to apply an incremental rotation, as you appear to be trying to do. Rather, you would store the orientation yourself (e.g. in matrix or quaternion form), apply an incremental rotation each update, and then upload the orientation to OpenGL (converting to matrix form first if necessary) for rendering purposes.

The basic idea you have looks right (although in reality, I imagine most fish make some effort to remain upright when swimming, a behavior that your current method won't necessarily exhibit). In order to make it work as you expect though, you'll need to do something like this:
// At initialization:matrix33 orientation = <initial orientation>;// For each update:// Compute the axis-angle rotation as in your current code, then:matrix33 rotation = matrix_from_axis_angle(axis, angle);orientation = rotation * orientation; // Assuming column-vector conventionorientation.orthogonalize(); // To correct for numerical error// Then, build a 4x4 matrix from the 3x3 orientation matrix and the current position:matrix44 transform = matrix_affine_transform(orientation, position);// And upload to OpenGL:glLoadMatrixf(transform.raw_data());

##### Share on other sites
First of all, thanks for the great reply!

Most important stuff first:
Quote:
 // At initialization:matrix33 orientation = ;// For each update:// Compute the axis-angle rotation as in your current code, then:matrix33 rotation = matrix_from_axis_angle(axis, angle);orientation = rotation * orientation; // Assuming column-vector conventionorientation.orthogonalize(); // To correct for numerical error// Then, build a 4x4 matrix from the 3x3 orientation matrix and the current position:matrix44 transform = matrix_affine_transform(orientation, position);// And upload to OpenGL:glLoadMatrixf(transform.raw_data());

I want to make sure I understand the concept:
1) Calculate the axis to rotate about and the angle (for the complete rotation?)
2) Build rotation matrix based on first step
3) Apply the rotation to the (old/current) orientation, to get a matrix representing the new orientation
4) Build transformation matrix from new orientation + current position
5) Apply to object

3 questions:
1: Why is it necessary to apply the orientation to the position? (Why) Wouldn't it be enough to just load the orientation matrix?
2: How exactly does the matrix_form_axis_angle look? (column-vector)
3: How exactly does the matrix_affine_transformation look?

Quote:
 There's not quite enough information there to tell you why your current code isn't working, mostly because we don't know in what context glRotatef() is being used. (That is, what's the state of the matrix stack when it's invoked? Are you saving the orientation somehow after it's been modified, e.g. using glGetFloatv()? And so on.)

I didn't want to just throw my code in here and go "fix my code", but the stack looks as follows:
glLoadIdentity();//Do view transformation/rotationsglPushMatrix();//Do motion translationglPushMatrix();glRotatef(..);glBegin(...);......glEnd();glPopMatrix();glPopMatrix();

Quote:
 The basic idea you have looks right (although in reality, I imagine most fish make some effort to remain upright when swimming, a behavior that your current method won't necessarily exhibit)

Yeah I thought about that, but I was even more clueless as how to achieve that - small steps :)

##### Share on other sites
Quote:
 1: Why is it necessary to apply the orientation to the position? (Why) Wouldn't it be enough to just load the orientation matrix?
The orientation isn't applied to the position (see my answer to question 3, below).
Quote:
 2: How exactly does the matrix_form_axis_angle look? (column-vector)
Google 'axis angle matrix', and you should find plenty of examples. As for matrix majorness and notational convention, there are really only two ways the matrix can be arranged (they're transposes of each other). Ideally you would find an example that stated what conventions are being used, but unfortunately there's a lot of tutorials and code examples out there that don't do this. If you're uncertain whether you have it right or not, you can always post your implementation here for people to take a look at.
Quote:
 3: How exactly does the matrix_affine_transformation look?
All it does (essentially) is create a 4x4 matrix, set it to identity, load the 3x3 matrix into the upper-left 3x3 portion, and load the translation vector into the fourth row or column (assuming you're using OpenGL notational convention, you'd load it into the fourth column).
Quote:
Quote:
 The basic idea you have looks right (although in reality, I imagine most fish make some effort to remain upright when swimming, a behavior that your current method won't necessarily exhibit)
Yeah I thought about that, but I was even more clueless as how to achieve that - small steps :)
Actually, the 'staying upright' version can actually be *easier* to implement than the 'arbitrary orientation' version; since all you really need to track is a pair of angles (yaw and pitch), you can dispense with doing the math manually and just use OpenGL's built-in math functions, as you were doing previously.

I'd stick with the 'arbitrary orientation' method though, regardless, since you'll probably get more realistic behavior if you first rotate into alignment with the direction vector, and then apply a 'stabilizing' rotation to keep the fish more or less upright.

##### Share on other sites
Okay, I implemented the new calculation but it scared my fish away :(
(translated out of the scene I guess)

First, I found the axis-angle matrix (Rodrigues' rotation matrix) (http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm)

So basically what happens is that without the rotation, my fish is in the scene moving normally. With the rotation applied however, is gone. I checked the matrix math twice (might still have missed something though). Anyways, here is the fish code and I hope you can spot the problem.

Here is the code for my fish class:
calculateTrajectory is called whenever the fish should swim to a new position
preFrame is called after calTraj and before draw.
#include "h.h"using namespace std;Fish::Fish(TriangleMesh* model) {	this->model = model;	this->trajectorySet = false;	this->avoidState = false;	this->heading = new Vector(0.0, 0.0, 1.0);	this->oldStopPosition = position;	this->orientation = new Matrix33(	1.0, 0.0, 0.0,										0.0, 1.0, 0.0,										0.0, 0.0, 1.0	);	axisAngleToRotation(orientation, new Vector(0.0, 0.0, 1.0), 0);	this->transformation = new Matrix44(1.0, 0.0, 0.0, 0.0,										0.0, 1.0, 0.0, 0.0,										0.0, 0.0, 1.0, 0.0,										0.0, 0.0, 0.0, 1.0);}void Fish::calculateTrajectory(Vector* newPosition, float speed) {	displacement = subtractVectors(newPosition, position);	travelTime = displacement->length() / speed;	startTime = Clock::getTime();	startPosition = position;	stopPosition = newPosition;	trajectorySet = true;	if (!sameVectors(oldStopPosition, stopPosition)) {		// |v x w | = |v||w|sin(a)		// a = sin-1 ( | v x w | / |v||w| )		turnAxis = crossProductNew(heading, displacement);		double angleRad = asin( turnAxis->length() / (heading->length() * displacement->length()));		double angleDeg =  angleRad * (180 / M_PI);		turnAngle = angleDeg;		heading = displacement;		turnAxis->normalize();		Matrix33* rotation = new Matrix33( 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); //Identity		axisAngleToRotation(orientation, turnAxis, turnAngle);		Matrix33* tmp = new Matrix33(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);		matrix33Mul(rotation, orientation, tmp);		orientation = tmp;	//	orientation.ortho();		affineTransformation(transformation, orientation, position);	//	affineTransformation(transformation, orientation, new Vector(0.0, 0.0, 0.0));		cout << "Orientation matrix" << endl;		orientation->print();	}}void Fish::preFrame(double time) {	if (trajectorySet) {		double elapsedTime = time-startTime;		if (elapsedTime > travelTime) {			position = stopPosition;			if (avoidState) {				avoidState = false;			}		} else {			Vector* currentDisplacement = scaleVectorNew(displacement, elapsedTime / travelTime);			position = addVectors(startPosition, currentDisplacement);		}	}}void Fish::draw() {	glPushMatrix();		glColor3f(colorR, colorB, colorG);		// Set heading		if (trajectorySet) {			GLfloat data[4][4] = {	{transformation->a, transformation->e, transformation->i, transformation->m},									{transformation->b, transformation->f, transformation->j, transformation->n},									{transformation->c, transformation->g, transformation->k, transformation->o},									{transformation->d, transformation->h, transformation->l, transformation->p}								};//			glLoadTransposeMatrixf(&data[4][4]);			glLoadMatrixf(&data[4][4]);		}		//Adjust model to fit scene		glScalef(5, 5, 5);		glBegin(GL_TRIANGLES);		for (unsigned int i = 0; i < model->faces.size(); i++) {			/* First Vertex */			glNormal3f(	model->normals[model->faces.vn[0]].x,						model->normals[model->faces.vn[0]].y,						model->normals[model->faces.vn[0]].z			);			glVertex3f(	model->verts[model->faces.v[0]].x,						model->verts[model->faces.v[0]].y,						model->verts[model->faces.v[0]].z			);			/* Second Vertex */			glNormal3f(	model->normals[model->faces.vn[1]].x,						model->normals[model->faces.vn[1]].y,						model->normals[model->faces.vn[1]].z			);			glVertex3f(	model->verts[model->faces.v[1]].x,						model->verts[model->faces.v[1]].y,						model->verts[model->faces.v[1]].z			);			/* Third Vertex */			glNormal3f(	model->normals[model->faces.vn[2]].x,						model->normals[model->faces.vn[2]].y,						model->normals[model->faces.vn[2]].z						);			glVertex3f(	model->verts[model->faces.v[2]].x,						model->verts[model->faces.v[2]].y,						model->verts[model->faces.v[2]].z			);		}		glEnd();	glPopMatrix();}Fish::~Fish() {}

For my matrix calculations:
#include "h.h"void axisAngleToRotation(Matrix33* m, Vector* v, GLfloat a) {	GLfloat s = sin(a) * (180 / M_PI);	GLfloat cplus = cos(a) * (180 / M_PI);	GLfloat cminus = 1 - cplus;	//Rodrigues' rotation matrix	m->a = cplus + (v->x*v->x) * cminus;	m->b = v->x * v->y * cminus + v->z * s;	m->c = v->x * v->z * cminus - v->y * s;	m->d = v->x * v->y * cminus - v->z * s;	m->e = cplus + (v->y * v->y) * cminus;	m->f = v->y * v->z * cminus + v->x * s;	m->g = v->x * v->z * cminus + v->y * s;	m->h = v->y * v->z * cminus - v->x * s;	m->i = cplus +(v->z * v->z) * cminus;}void affineTransformation(Matrix44* m, Matrix33* r, Vector* v) {	m->a = r->a; m->e = r->d; m->i = r->g; m->m = v->x;	m->b = r->b; m->f = r->e; m->j = r->h; m->n = v->y;	m->c = r->c; m->g = r->f; m->k = r->i; m->o = v->z;	m->d = 0;	 m->h = 0;	  m->l = 0;	   m->p = 1;}void matrix33Mul(Matrix33* m1, Matrix33* m2, Matrix33* dest) {	dest->a = m1->a * m2->a + m1->d * m2->b + m1->g * m2->c;	dest->b = m1->b * m2->a + m1->e * m2->b + m1->h * m2->c;	dest->c = m1->c * m2->a + m1->f * m2->b + m1->i * m2->c;	dest->d = m1->a * m2->d + m1->d * m2->e + m1->g * m2->f;	dest->e = m1->b * m2->d + m1->e * m2->e + m1->h * m2->f;	dest->f = m1->c * m2->d + m1->f * m2->e + m1->i * m2->f;	dest->g = m1->a * m2->g + m1->d * m2->h + m1->g * m2->i;	dest->h = m1->b * m2->g + m1->e * m2->h + m1->h * m2->i;	dest->i = m1->c * m2->g + m1->f * m2->h + m1->i * m2->i;}

For the matrix33 class (same for 44):
#include "h.h"using namespace std;Matrix33::Matrix33(	GLfloat a, GLfloat d, GLfloat g,					GLfloat b, GLfloat e, GLfloat h,					GLfloat c, GLfloat f, GLfloat i) {	this->a = a; this->d = d; this->g = g;	this->b = b; this->e = e; this->h = h;	this->c = c; this->f = f; this->i = i;}Matrix33::~Matrix33() {	// TODO Auto-generated destructor stub}void Matrix33::print() {	cout << "| " << a << "  " << d << "  " << g << " |" << endl;	cout << "| " << b << "  " << e << "  " << h << " |" << endl;	cout << "| " << c << "  " << f << "  " << i << " |" << endl;}

##### Share on other sites
I didn't examine all the code carefully, but I did notice this:
axisAngleToRotation(orientation, turnAxis, turnAngle);
Should that be rotation rather than orientation?

Also, based on a quick look over your code, it looks to me like you might be making some basic programming errors with respect to C++. Are you perhaps coming to C++ from a garbage-collected language such as C# or Java? Do you have much familiarity with the C++ memory model?

I realize this is a little off-topic, but if you're leaking memory on a per-frame basis (which it looks like you may be), that's probably something you'll want to fix.

##### Share on other sites
Quote:
 I didn't examine all the code carefully, but I did notice this:axisAngleToRotation(orientation, turnAxis, turnAngle);Should that be rotation rather than orientation?

Of cause it should! Thanks

Quote:
 Are you perhaps coming to C++ from a garbage-collected language such as C# or Java? Do you have much familiarity with the C++ memory model?

That obvious ? :) Yes, Java's the one. I'm just as new to C++ as I am to OpenGL, so thanks for pointing that out - and it needs fixing naturally; can't have bad code.

##### Share on other sites
Quote:
 I didn't examine all the code carefully, but I did notice this:axisAngleToRotation(orientation, turnAxis, turnAngle);Should that be rotation rather than orientation?

Of cause it should! Thanks

Quote:
 Are you perhaps coming to C++ from a garbage-collected language such as C# or Java? Do you have much familiarity with the C++ memory model?

That obvious ? :) Yes, Java's the one. I'm just as new to C++ as I am to OpenGL, so thanks for pointing that out - and it needs fixing naturally; can't have bad code.

EDIT: Still no go, same result

##### Share on other sites
Quote:
 Yes, Java's the one. I'm just as new to C++ as I am to OpenGL, so thanks for pointing that out - and it needs fixing naturally; can't have bad code.
Yeah, unfortunately, C++ is laden with pitfalls, and if you're used to different programming paradigms or language semantics, it's easy to fall right into them. (Actually, it's pretty easy to fall into them even if you've been programming in C++ for quite a while.)

The main thing you'll want to keep in mind is that C++ has no language-level garbage collection. What you create with new or new[], you are expected to release (in one way or another) using delete or delete []. Anything that isn't released explicitly remains in memory (in an abstract sense, at least) until the program terminates. If you continually create new objects without ever deleting them, your application will consume more and more memory the longer it runs.

For resources that do need to be allocated dynamically, it's common to automate the release of those resources using the RAII idiom. However, for the type of thing you're doing, the solution is to work with values rather than references (pointers, in this case).
Quote:
 EDIT: Still no go, same result
I think there's probably too much going on in your code for me to pick out the problem easily. I think it's at least a possibility that some of the pointer and memory mis-handling may be a contributing factor, so I think it might be a good idea to try and clean some of that up before proceeding further.

If you have questions on the C++ side of things, you can always post in one of the other forums, and people will be happy to help.

Also, what development tools are you using? If you have a debugger available, you'll want to become familiar with it, as that's usually the best way to solve problems like this one.

##### Share on other sites
Quote:
 What you create with new or new[], you are expected to release (in one way or another) using delete or delete [].

That sounds simple enough, but I imagine the trick is to figure out what to release when. Say for example this:

foo(Bar* f) {   Bar tmp = new Bar(..); //temp holder for calculations   //calculations   f = &tmp;   return;}

Will deleting tmp before return result in f pointing to, well it would point to the same address, but will new stuff be allocated to that address, giving me wired results?

Anyway, I've picked up a C++ book, so I will read up on and rewrite the code from scratch step by step (my guess is that cleaning up, inserting deletes in one step will go BOOM and take a lot longer to get running again).

Quote:
 Also, what development tools are you using?

Eclipse and I've spend plenty time in the debugger :)

Well, again, thanks a lot for taking the time to help with my rookie mistakes!

-Niux

##### Share on other sites
Cool are you making a 3d fishing game?

##### Share on other sites
Well, no :) This is for an introductory OpenGL class I'm taking, where we need to implement a Boids model in 3D. However, the cool thing is that a part of the project is to make it in 3D stereo (Avatar style). Plus we should be able to interact with the shoal of fish with a Wii-mote, so we get the 3D interaction feel. But maybe I'll make it a game - I mean, 3D stereo Wii-mote controlled fishing game sounds alright ;)

##### Share on other sites
Quote:
 That sounds simple enough, but I imagine the trick is to figure out what to release when. Say for example this:foo(Bar* f) { Bar tmp = new Bar(..); //temp holder for calculations //calculations f = &tmp; return;} Will deleting tmp before return result in f pointing to, well it would point to the same address, but will new stuff be allocated to that address, giving me wired results?
Problems with the above code:

foo(Bar* f) {    // This won't compile. 'tmp' is of type 'Bar', and the return value of new in this case is    // of type 'Bar*'. Since the latter cannot be converted to the former (unless you've    // written a constructor that performs the conversion, which would be both    // confusing and unusual), the compiler will generate an error here.    Bar tmp = new Bar(..);    // This will compile, but it won't do what you're expecting. The argument 'f' (a pointer to a Bar)    // is passed by value, so it's essentially a temporary variable local to the function. As such,    // assigning a value to 'f' here will have no effect outside of this function.    // To get the behavior I think you're going for here, you would need to pass 'f' by reference    // (i.e. Bar*& f), and then perform the assignment like this: 'f = new Bar(...);' Also, you    // probably wouldn't want to delete anything here, since you want the newly created 'Bar'    // object to persist. However, this way of doing things raises potentially confusing ownership    // issues, and isn't really recommended. In fact, in modern C++, working with raw pointers    // in this way is best avoided whenever possible. (The way to avoid it is to use RAII containers -    // such as std::vector and boost/tr1::shared_ptr - that have clear ownership semantics and    // (usually) take care of any necessary clean-up for us.)    // Isn't C++ fun? :-)    f = &tmp;    // This is valid, but superfluous (if the function doesn't return anything, you don't need to    // include a return statement).    return;}

Having written all that, the real solution to the problems with your code is not to use pointers at all. With a few possible exceptions that I won't go into here, none of the code in your program that deals with matrices or vectors should be using pointers, new, or delete. Matrix and vector objects should not be passed around by pointer, but rather by value or (possibly constant) reference. Nowhere in your code should you be writing anything like this:
Matrix44* matrix = new Matrix44(...);
Instead, you would just write this:
Matrix44 matrix(...);
Just out of curiosity, is it part of the assignment that the program be written in C++? Or is the choice of language up to you?

##### Share on other sites
Quote:
 Just out of curiosity, is it part of the assignment that the program be written in C++? Or is the choice of language up to you?

Part of the assignment. However I don't mind - should be able to write C/C++ before I get a BS in CS - unfortunately, my university has Java as the official language of choice.

Quote:
 Matrix44* matrix = new Matrix44(...);Instead, you would just write this:Matrix44 matrix(...);

Yes - I picked up a copy of 'The C++ Programming Language' and started reading up. The code now looks like this:
/* * Fish.h * *  Created on: Mar 8, 2010 *      Author: Mark */class Fish {public:	Fish();	Fish(TriangleMesh* model);	void calculateNewTrajectory(Vector3* newPosition, float speed);	void preFrame(double time);	void draw();	virtual ~Fish();private:	void init();	void drawFishModel();	void drawFish();	TriangleMesh* model;	Vector3 position;	Vector3 stopPosition;	Vector3 startPosition;	Vector3 displacement;	double travelTime;	double startTime;	bool trajectorySet;	bool avoidState;	bool withModel;};

/* * Fish.cpp * *  Created on: Mar 8, 2010 *      Author: Mark */#include "h.h"using namespace std;Fish::Fish() {	withModel = false;	init();}Fish::Fish(TriangleMesh* model) {	this->model = model;	withModel = true;	init();}void Fish::init() {	position = Vector3(0.0, 0.0, 0.0);	//Prevents read from uninitialised variables, if calculateNewTrajectory	//has never bin called.	trajectorySet = false;}/* Pre-calculate everything needed to move fish from A to B */void Fish::calculateNewTrajectory(Vector3* newPosition, float speed) {	//Find displacement vector (vector fish will swim along)	subtractVectors(&displacement, newPosition, &position);	stopPosition = *newPosition; //Copy vector	startPosition = position;	travelTime = displacement.length() / speed;	startTime = Clock::getTime();	trajectorySet = true;}/* Move the fish if a trajectory is set */void Fish::preFrame(double time) {	if(trajectorySet) { //Check that the fish has somewhere to go			double elapsedTime = time - startTime;			if (elapsedTime > travelTime) { //Is the movement done?				position = stopPosition; //Copy vector				//If this movement was an 'avoid other fish' movement				//exit the avoid state when movement is done				if (avoidState) {					avoidState = false;				}			} else { //If movement is not done, continue along displacement vector				Vector3 currentDisplacement;				//Calculate how far the fish have move until now, based on time				scaleVector(&currentDisplacement, &displacement, elapsedTime / travelTime);				//Update the fish' position				addVectors(&position, &startPosition, &currentDisplacement);			}	}}/* Draw the fish and rotate to face in the direction of movement. * Also to basic fish animation. */void Fish::draw() {	//Rotate with respect to local coordinate system	//Do the drawing	glPushMatrix();		if (!withModel) {			drawFish();		} else {			drawFishModel();		}	glPopMatrix();}/* Draws a fish (sort of) if no model is used */void Fish::drawFish() {	glutWireSphere(0.5, 5, 5);}/* Draws a fish if a model is loaded */void Fish::drawFishModel() {	glBegin(GL_TRIANGLES);	for (unsigned int i = 0; i < model->faces.size(); i++) {		/* First Vertex */		glNormal3f(	model->normals[model->faces.vn[0]].x,					model->normals[model->faces.vn[0]].y,					model->normals[model->faces.vn[0]].z		);		glVertex3f(	model->verts[model->faces.v[0]].x,					model->verts[model->faces.v[0]].y,					model->verts[model->faces.v[0]].z		);		/* Second Vertex */		glNormal3f(	model->normals[model->faces.vn[1]].x,					model->normals[model->faces.vn[1]].y,					model->normals[model->faces.vn[1]].z		);		glVertex3f(	model->verts[model->faces.v[1]].x,					model->verts[model->faces.v[1]].y,					model->verts[model->faces.v[1]].z		);		/* Third Vertex */		glNormal3f(	model->normals[model->faces.vn[2]].x,					model->normals[model->faces.vn[2]].y,					model->normals[model->faces.vn[2]].z					);		glVertex3f(	model->verts[model->faces.v[2]].x,					model->verts[model->faces.v[2]].y,					model->verts[model->faces.v[2]].z		);	}	glEnd();}Fish::~Fish() {}

My vector/matrix functions looks like this

addVectors(&position, &startPosition, &currentDisplacement);......void addVectors(Vector3* dest, Vector3* v1, Vector3* v2) {	dest->x = v1->x+v2->x;	dest->y = v1->y+v2->y;	dest->z = v1->z+v2->z;}

As you can see I'm not done with the rewriting yet. I'm not sure whether passing pointers
to the
addVectors
function is the best practise (I guess v1 and v2 should be constant references)? The only other thing that uses pointers is the
triangularMesh
, but since the model is the same for all fish objects - and never changed - it seems like a waste of memory to load a model for each fish.

Again, thanks for all the time your are spending on this - if I could rate you higher, I would.

[Edited by - Niux on March 9, 2010 6:43:30 AM]

##### Share on other sites
Quote:
 As you can see I'm not done with the rewriting yet. I'm not sure whether passing pointers to the addVectors function is the best practise (I guess v1 and v2 should be constant references)?
Passing by pointer as you're doing now will work fine, but as you note, it's more idiomatic to pass by reference (constant if the original object is not to be modified, otherwise non-constant).

Note also that when using references, you access member variables and functions using the . operator rather than the -> operator (which you may already know).

Also, C++ has support for operator overloading, which means that (provided you've implemented the appropriate operators), you don't have to write code like this:
Vector3 position;addVectors(&position, &startPosition, ¤tDisplacement);
Vector3 position = startPosition + currentDisplacement;
However, that's just syntactic sugar, more or less - what you have now should work fine.

##### Share on other sites
Okay, so the code should be clean now. Back to my problem.

What I've done except for cleaning code, since the orientation algorithm/calculation still doesn't work, is to only use the rotation matrix (axis-angle matrix / Rodriques' rotation matrix, lefthanded). Now the rotation seems to work right this way, but my fish is scaled alot(!) in one axis and pancaked in another axis.

Here's what happening:
1) Find crossProduct between current heading (initially (0,0,1)) and new displacement
2) Find rotation angle by: angle = sin-1(|v x w| / |v||w|) * (180 / M_PI)
3) Compose rotation matrix
4) Put rotation matrix on matrix stack

Notice, I'm not updating the heading (heading = normalize(displacement)) - if I do so, the fish is obfuscated even more (scaled into a wired shape).

I've taken a closer look at what I'm doing with the matrix stack (entire application):
Method:              Stack:      Comment:glLoadIdentity();    [I, ]glPushMatrix();      [I, I]glTranslatef(...);   [I, T]      Translate fish to its position in R^3glPushMatrix();      [I, T, T]glMultMatrix(...);   [I, T, TR]  Load the rotation matrix on the stack (*)glPuchMatrix();      [I, T, TR, ]glScalef(...);       [I, T, TR, S]  Adjust size of fishglRotatef(...);      [I, T, TR, S, R] Rotate fish to face positive Z after drawing

*) If i use glLoadMatrix(..); here, as I expected was the right thing, the fish ends
up in the face of the camera?

I looked at some actual angle,axis,rotation matrix values too see if they where the problem: (Fish is at (4, 0, 0) and starts moving towards (0, 0, 0))
turn Axis: (0, -1, 0)turn angle: 90rotation matrix:| -25.6727  -0  -51.2222 || 0          1  -0       || 51.2222  0  -25.6727   |

The axis and angle looks right to me - not sure about the matrix though.

Here is the code for vector cross product:
void crossProduct(Vector3& dest, Vector3& v1, Vector3& v2) {	/*	 * a	 d		bf - ce	 * b  x	 e  =	cd - fa	 * c	 f		ae - bd	 */	float a = v1.x;	float b = v1.y;	float c = v1.z;	float d = v2.x;	float e = v2.y;	float f = v2.z;	dest.x = (b*f) - (c*e);	dest.y = (c*d) - (f*a);	dest.z = (a*e) - (b*d);}

Here is the code snippet for axis-angle calculations:
	Vector3 turnAxis;	crossProduct(turnAxis, heading, displacement);	//Find angle	double angle = asin( turnAxis.length() / (heading.length() * displacement.length()));	angle *= (180 / M_PI);	turnAxis.normalize();	//Build axis-angle (rotation) matrix	rotation = Matrix33(	1.0, 0.0, 0.0,			        0.0, 1.0, 0.0,				0.0, 0.0, 1.0 );	axisAngleToRotation(rotation, turnAxis, angle);

Here is the code for building the rotation matrix:
void axisAngleToRotation(Matrix33& m, Vector3& v, GLfloat a) {	GLfloat s = sin(a) * (180 / M_PI);	GLfloat cplus = cos(a) * (180 / M_PI);	GLfloat cminus = 1 - cplus;	/* Rodrigues' rotation matrix, left handed, column major	 * (Given the vector = (x, y, z))	 *	 * |  c_plus + x^2 * c_minus    x*y*c_minus - x*s        x*z*c_minus + y*s       |	 * |  x*y*c_minus - z*s         c_plus + b^2 * c_minus   y*z*c_minus - x*s       |	 * |  x*z*c_minus - y*s         y*z*c_minus + x*s        c_plus + z^2 * c_minus  |	 *	 * Where s = sin(angle), c_plus = cos(angle), c_minus = 1-c_plus	 */	m.a = cplus + (v.x*v.x) * cminus;	m.b = v.x * v.y * cminus + v.z * s;	m.c = v.x * v.z * cminus - v.y * s;	m.d = v.x * v.y * cminus - v.z * s;	m.e = cplus + (v.y * v.y) * cminus;	m.f = v.y * v.z * cminus + v.x * s;	m.g = v.x * v.z * cminus + v.y * s;	m.h = v.y * v.z * cminus - v.x * s;	m.i = cplus +(v.z * v.z) * cminus;}

	GLfloat mat3[16]={	rotation.a, rotation.d, rotation.g, 1.0,						rotation.b, rotation.e, rotation.h, 1.0,						rotation.c, rotation.f, rotation.i, 1.0,						0.0, 		0.0,		0.0, 		1.0};	glMultTransposeMatrixf(mat3);

##### Share on other sites
There's a lot going on here, and a lot of things that look like they may be incorrect. For starters, I'll just point out a few things I noticed.

If this:
| -25.6727  -0  -51.2222 || 0          1  -0       || 51.2222  0  -25.6727   |
Is what you're getting for an axis-angle rotation of (90, [0, -1, 0]), then your axis-angle-to-matrix function is wrong. Each of the basis vectors of the resulting matrix should be unit-length, more or less, which clearly isn't the case here; also, a 90-degree rotation should leave you with an 'axis-aligned' orientation, which leads me to believe that there's an incorrect or missing degrees<->radians conversion somewhere.

I didn't proof the code for that function, but I'd recommend double-checking it very carefully against whatever reference you're using to make sure the math is right.

Next, this:
GLfloat mat3[16]={    rotation.a, rotation.d, rotation.g, 1.0,    rotation.b, rotation.e, rotation.h, 1.0,    rotation.c, rotation.f, rotation.i, 1.0,    0.0, 0.0, 0.0, 1.0};
Is incorrect; the right-most column should be [0, 0, 0, 1]T, not [1, 1, 1, 1]T.

Lastly, it doesn't seem to me that you're using the computed rotation matrix correctly. Unless you're reading back the OpenGL modelview matrix somewhere, you shouldn't ever be uploading the relative rotation directly to OpenGL. Rather, you need to track the orientation of the fish yourself (using another matrix), update it each frame using the relative rotation matrix, orthogonalize it to prevent drift, and then upload that matrix to OpenGL when it's time to render.

[Edited by - jyk on March 10, 2010 12:00:47 PM]

##### Share on other sites
Quote:
 Is what you're getting for an axis-angle rotation of (90, [0, -1, 0]), then your axis-angle-to-matrix function is wrong.

Quote:
 Is what you're getting for an axis-angle rotation of (90, [0, -1, 0]), then your axis-angle-to-matrix function is wrong. Each of the basis vectors of the resulting matrix should be unit-length, more or less, which clearly isn't the case here; also, a 90-degree rotation should leave you with an 'axis-aligned' orientation, which leads me to believe that there's an incorrect or missing degrees<->radians conversion somewhere.

Good, I'll look into these things.

Quote:
 Is incorrect; the right-most column should be [0, 0, 0, 1]T, not [1, 1, 1, 1]T.

Yes, sorry - I was just testing (1,1,1,1) to see the difference and forgot to set it back (when I wrote the post, I had (0,0,0,1) in mind)

Quote:
 Lastly, it doesn't seem to me that you're using the computed rotation matrix correctly. Unless you're reading back the OpenGL modelview matrix somewhere, you shouldn't ever be uploading the relative rotation directly to OpenGL. Rather, you need to track the orientation of the fish yourself (using another matrix), update it each frame using the relative rotation matrix, orthogonalize it to prevent drift, and then upload that matrix to OpenGL when it's time to render.

Okay - I've clearly misunderstood that part, and I can see the mistake now.

Again, thanks alot!

##### Share on other sites
Okay - minor update.

I found wrong radian/degree conversions, just as you said. The axis-angle matrix was fine though. I updated the code to remember the orientation of the fish and now orientation*relative rotation is uploaded to opengl. That fixed the scaling problems and every thing looks more correct now. Only thing is the orientation of the fish isn't right.

When the program starts, the fish is a (4, 0, 0)T (facing positive Z) and begins to move towards (0, 0, 0)T. That works correctly - the fish rotates from facing positive Z to negative X. However when the fish gets to (0, 0, 0)T, it starts to move towards (5, 5, 5)T - this is where the orientation starts to be wrong. So I printed the matrices and axis/angles:
//Fish is at (0, 0, 4) starts moving towards (0, 0, 0)new position: (0, 0, 0)//Find axis angleturn Axis: (0, -4, 0) //See belowturn angle: 90//axis-angle to rotation matrixrelative rotation:| 6.12303e-17  -0  -1 || 0  1  -0 || 1  0  6.12303e-17 |// orientation = rotation * orientation - actually I haven't 2x checked this function EDIT: now I have, it's correctnew orientation:| 6.12303e-17  0  -1 || 0  1  0 || 6.12303e-17  0  -1 |//Update heading to displacement.normalize()old heading: (0, 0, 1)new heading: (-1, 0, 0)//Fish is at (0, 0, 0) (facing negative X) starts moving towards (5, 5, 5)new position: (5, 5, 5)//Find axis angleturn Axis: (0, 5, -5) //See belowturn angle: 54.7356//axis-angle to rotation matrixrelative rotation:| 0.57735  0.57735  0.57735 || -0.57735  0.788675  -0.211325 || -0.57735  -0.211325  0.788675 |// orientation = rotation * orientationnew orientation:| 7.07027e-17  0.57735  -1.1547 || -5.37597e-17  0.455342  0.877991 || 1.88314e-17  -0.429558  -0.30755 |//Update heading to displacement.normalize()old heading: (-1, 0, 0)new heading: (0.57735, 0.57735, 0.5773

About the turn axis: I take the cross product between current heading (normalized) and the new displacement vector (newPosition - oldPosition) - NOT normalized.
EDIT: I normalize the turnAxis vector before passing it to the axisAngleToRotationMatrix-function. If I don't, I get strange scale-transformations when applying the new orientation matrix.

For comparison I staged an example of the fish moving from (0, 0, 0)T (facing positive Z) to (5, 5, 5)T as the first move, ie. with a correct orientation.

new position: (5, 5, 5)turn Axis: (-5, 5, 0)turn angle: 54.7356relative rotation:| 0.788675  -0.211325  0.57735 || -0.211325  0.788675  0.57735 || -0.57735  -0.57735  0.57735 |new orientation:| 0.788675  -0.211325  0.57735 || -0.166667  0.833333  0.455342 || -0.359117  -0.359117  -0.0188747 |old heading: (0, 0, 1)new heading: (0.57735, 0.57735, 0.57735)

The turn axis' are different thus the relative rotation matrix is different. I'm still trying to figure out why the turnAxis is different, but I posted this since experienced eyes might be able to spot the error :)

EDIT: Doh - of cause the turnAxis is different, then initial heading is different (negative X vs. positive Z)

EDIT2: I did another test - keeping the fish axis-aligned:
Facing:	From:		To:		Result:	Result facing:	Expected facing:+ Z	(4, 0, 0)	(4, 0, 0)	Good		- X		- X- X	(0, 0, 0)	(0, 0, 4)	Bad		+ X		+ Z+ X	(0, 0, 4)	(0, 0, 0)	Bad		+ Z		- Z+ Z	(0, 0, 0)	(0, 4, 0)	Bad		- Y		+ Y- Y	(0, 4, 0)	(0, 0, 0)	Bad		+ Z		- Y+ Z	(0, 0, 0)	(0, 4, 0)	Bad		+ Z		+ Y+ Z 	(0, 4, 0)	(0, 0, 0)	Bad		+ Z		- Y+ Z	(0, 0, 0)	(0, 0, 4)	Bad		+ Y		+ Z+ Y	(0, 0, 4)	(0, 0, 0)	Bad		+ Z		- Z

The matrix from the second step looks like this:
|0 0 1|
|0 1 -0|
|-1 0 0|

Applying that to the old heading vector (-1, 0, 0)T, gives the expected result (0, 0, 1)T.

[Edited by - Niux on March 11, 2010 6:40:02 AM]

##### Share on other sites
Quote:
 Original post by NiuxI found wrong radian/degree conversions, just as you said. The axis-angle matrix was fine though.

Quote:
 Original post by Niux//axis-angle to rotation matrixrelative rotation:| 0.57735 0.57735 0.57735 || -0.57735 0.788675 -0.211325 || -0.57735 -0.211325 0.788675 |// orientation = rotation * orientationnew orientation:| 7.07027e-17 0.57735 -1.1547 || -5.37597e-17 0.455342 0.877991 || 1.88314e-17 -0.429558 -0.30755 |

The last matrix is not orthonormal, hence not a valid rotation. Either the matrix orientation or the matrix multiplication is wrong.

Cheers,
fng

• 11
• 9
• 11
• 9
• 12