Rotate object to face towards point
Author
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 ;)
Quote:That sounds simple enough, but I imagine the trick is to figure out what to release when. Say for example this:Problems with the above code: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?
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(...);Matrix44 matrix(...);
Author
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(¤tDisplacement, &displacement, elapsedTime / travelTime); //Update the fish' position addVectors(&position, &startPosition, ¤tDisplacement); } }}/* 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, ¤tDisplacement);......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
addVectorstriangularMeshAgain, 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]
Quote:As you can see I'm not done with the rewriting yet. I'm not sure whether passing pointersPassing 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).
to thefunction is the best practise (I guess v1 and v2 should be constant references)?addVectors
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;
Author
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):
*) 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))
The axis and angle looks right to me - not sure about the matrix though.
Here is the code for vector cross product:
Here is the code snippet for axis-angle calculations:
Here is the code for building the rotation matrix:
Here is the code for loading the matrix on the stack:
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;}Here is the code for loading the matrix on the stack:
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);
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:
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:
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]
If this:
| -25.6727 -0 -51.2222 || 0 1 -0 || 51.2222 0 -25.6727 |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};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]
Author
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!
Author
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:
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.
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:
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]
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.5773About 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 - ZThe 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]
Quote:Original post by Niux
I 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 matrix
relative rotation:
| 0.57735 0.57735 0.57735 |
| -0.57735 0.788675 -0.211325 |
| -0.57735 -0.211325 0.788675 |
// orientation = rotation * orientation
new 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
This topic is closed to new replies.
Advertisement
Popular Topics
Advertisement
