#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
//
// VectorStack Class
//
// Implements a stack of 4-vectors that can contain points, vectors,
// quaternions, and matrices. Similar to the OpenGL Matrix stack except
// at a vector level granularity.
// This interface is inspired by Forth, a simple stack-base language.
// This implementation is not complete. There are many vector, point,
// and quaternion operations that would fit into the interface. The ones
// here were chose to demonstrate the idea.
//
#define VECTOR_STACK_DEPTH 8
typedef float VECTOR[4];
#ifndef M_PI
const float M_PI = 3.14159265358979323846;
#endif
VECTOR gv_identity = {0.0,0.0,0.0,1.0};
VECTOR gv_x = {1.0,0.0,0.0,0.0};
VECTOR gv_y = {0.0,1.0,0.0,0.0};
VECTOR gv_z = {0.0,0.0,1.0,0.0};
class VectorStack
{
VECTOR m_stack[VECTOR_STACK_DEPTH];
int m_index;
public:
enum {
X = 0, Y = 1, Z = 2, W = 3,
};
VectorStack () {
m_index = 0;
}
void copy (VECTOR dst, VECTOR src) {
dst[X] = src[X];
dst[Y] = src[Y];
dst[Z] = src[Z];
dst[W] = src[W];
}
void check_stack (int push, int pop) {
// check the stack has room for the given number of pushes and pops
if (m_index + push >= VECTOR_STACK_DEPTH) {
printf("Stack overflow!");
exit(-1);
}
if (m_index - pop < 0) {
printf("Stack underflow!");
exit(-1);
}
}
void push (VECTOR vec) {
// push a vector onto the stack
check_stack(1,0);
copy(m_stack[m_index++],vec);
}
void push (float x, float y, float z, float w) {
// push a vector
VECTOR v0;
v0[X] = x;
v0[Y] = y;
v0[Z] = z;
v0[W] = w;
push(v0);
}
void push (float x, float y, float z) {
// push a vector with w set to 0.0
push(x,y,z,0.0);
}
void push (VECTOR vec, float w) {
// push a vector and override the w
push(vec[0],vec[1],vec[2],w);
}
void insert (VECTOR vec, int index) {
// insert a vector into nth position on the stack
check_stack(1,index);
if (index == 0) {
push(vec);
}
else {
memmove(&m_stack[m_index-index+1],
&m_stack[m_index-index],
index*sizeof(m_stack[0])
);
copy(m_stack[m_index-index],vec);
m_index++;
}
}
void pop (VECTOR dest) {
// pop the top vector on the stack
check_stack(0,1);
copy(dest,m_stack[--m_index]);
}
void pop () {
// pop the top vector on the stack
check_stack(0,1);
m_index--;
}
float * top (int index) {
// return a peek at the nth vector on the stack
check_stack(0,index+1);
return m_stack[m_index-(index+1)];
}
float * top () {
// return a peek at the top vector on the stack
return top(0);
}
void dup () {
// duplicate the top vector on the stack
// (v0 -- v0 v0)
check_stack(1,1);
copy(m_stack[m_index],m_stack[m_index-1]);
m_index++;
}
void swap () {
// swap the top two vectors on the stack
// this could *really* be optimized :)
// (v0 v1 -- v1 v0)
VECTOR v1;
pop(v1);
insert(v1,1);
}
void rotate () {
// rotate the top three vectors on the stack
// ( v0 v1 v2 -- v2 v0 v1 )
VECTOR v2;
pop(v2);
insert(v2,2);
}
void over2 () {
// grab the second vector on the stack and push it
// ( v0 v1 v2 -- v0 v1 v2 v0)
float * v0 = top(2);
push(v0);
}
void print () {
// pop and print the top vector on the stack
VECTOR vec;
pop(vec);
printf("{ %4.4f, %4.4f, %4.4f, %4.4f }\n", vec[X], vec[Y], vec[Z], vec[W]);
}
void dump (const char * msg = NULL) {
// print out the stack with an option message
printf("--------------------------------------------------------------------\n");
if (msg)
printf("%s\n",msg);
for (int ii = 0; ii < m_index; ++ii) {
float * vec = m_stack[ii];
printf("%d: { %4.4f, %4.4f, %4.4f, %4.4f }\n", ii, vec[X], vec[Y], vec[Z], vec[W]);
}
printf("--------------------------------------------------------------------\n");
}
void clear () {
// clear the stack
m_index = 0;
}
void multiply () {
// element-wise multiply the top two vectors on the stack
VECTOR v0, v1, res;
pop(v0);
pop(v1);
for (int i = 0; i < 3; ++i)
res[i] = v0[i] * v1[i];
res[W] = 1.0;
push(res);
}
void add () {
// add the top two vectors on the stack
VECTOR v0,v1;
pop(v0);
pop(v1);
for (int i = 0; i < 3; ++i)
v0[i] += v1[i];
v0[W] = 1.0;
push(v0);
}
void subtract () {
// subtract the top two vectors on the stack
VECTOR v0,v1;
pop(v1);
pop(v0);
for (int i = 0; i < 3; ++i)
v0[i] -= v1[i];
v0[W] = 0.0;
push(v0);
}
float dot () {
// dot the top two vectors on the stack
VECTOR v0, v1;
pop(v0);
pop(v1);
float res = 0.0;
for (int i = 0; i < 3; ++i)
res += v0[i] * v1[i];
return res;
}
void cross () {
// cross the top two vectors on the stack
VECTOR v0, v1, res;
pop(v1);
pop(v0);
res[X] = v0[Y]*v1[Z] - v0[Z]*v1[Y];
res[Y] = v0[X]*v1[Z] - v0[Z]*v1[X];
res[Z] = v0[X]*v1[Y] - v0[Y]*v1[X];
res[W] = 0.0;
push(res);
}
float length () {
// return the length of the vector
check_stack(0,1);
float * v0 = top();
float x=v0[X],y=v0[Y],z=v0[Z];
return sqrt(x*x + y*y + z*z);
}
void scale (float scalar) {
// multiply the vector by a scalar
float * v0 = top();
for (int i = 0; i < 3; ++i)
v0[i] *= scalar;
}
void normalize (float scalar) {
// normalize the vector to a scalar
float * v0 = top();
float len = length();
if (len != 0.0)
scale(scalar/len);
}
void normalize () {
normalize(1.0);
}
void matrix_from_forward_up () {
// given normalized forward and up vectors, calculate a rotation matrix.
// ( forward up )
dup(); // (forward up up)
rotate(); // (up forward up)
cross(); // (up right)
normalize(-1.0); // (up nleft)
swap(); // (nleft up)
dup(); // (nleft up up)
over2(); // (nleft up up nleft)
cross(); // (nleft up backward)
scale(-1.0); // (nleft up forward)
}
void axis_angle_to_quaternion () {
// given an vector axis with angle in w, calculate the representative quaternion
// q = i ( x * sin(a/2)) + j (y * sin(a/2)) + k ( z * sin(a/2)) + cos(a/2)
VECTOR v0,q0;
pop(v0);
float x=v0[X], y=v0[Y], z=v0[Z], w=v0[W];
float half_angle = w*0.5;
float cos_ha = cos(half_angle);
float sin_ha = sin(half_angle);
q0[X] = x*sin_ha;
q0[Y] = y*sin_ha;
q0[Z] = z*sin_ha;
q0[W] = cos_ha;
push(q0);
}
void quaternion_to_rotation_matrix () {
// given a quaternion, calculate a rotation matrix
VECTOR q0,v0;
pop(q0);
float x=q0[X],y=q0[Y],z=q0[Z],w=q0[W];
float xx = x*x, yy = y*y, zz = z*z;
float xy = x*y, wx = w*x, yz = y*z, wy = w*y, xz = x*z, wz = w*z;
push(1.0 - 2.0*(yy + zz), 2.0*(xy - wz), 2.0*(xz + wy));
push(2.0*(xy + wz), 1.0 - 2.0*(xx - zz), 2.0*(yz - wx));
push(2.0*(xz - wy), 2.0*(yz + wx), 1.0 - 2.0*(xx + yy));
}
void quaternion_x_vector () {
// given a quaternion, extract just the x axis
VECTOR q0,v0;
pop(q0);
float x=q0[X],y=q0[Y],z=q0[Z],w=q0[W];
push(1.0 - 2.0*(y*y + z*z), 2.0*(x*y - w*z), 2.0*(x*z + w*y));
}
void quaternion_y_vector () {
// given a quaternion, extract just the y axis
VECTOR q0,v0;
pop(q0);
float x=q0[X],y=q0[Y],z=q0[Z],w=q0[W];
push(2.0*(x*y + w*z), 1.0 - 2.0*(x*x - z*z), 2.0*(y*z - w*x));
}
void quaternion_z_vector () {
// given a quaternion, extract just the z axis
VECTOR q0,v0;
pop(q0);
float x=q0[X],y=q0[Y],z=q0[Z],w=q0[W];
push(2.0*(x*z - w*y), 2.0*(y*z + w*x), 1.0 - 2.0*(x*x + y*y));
}
};
void test_vector_stack () {
// perform some simple operations with the VectorStack to demonstrate and
// test it.
VectorStack vs;
//
// Construct an identity matrix by constructing a rotation matrix from
// the axes.
//
vs.push(gv_z);
vs.push(gv_y);
vs.matrix_from_forward_up(); // convert the two axes into a rotation matrix
vs.dump("matrix_from_forward_up() => (should look like 3x4 identity)");
vs.clear();
//
// Simulating getting an angle between an object's heading and its
// desination.
//
float heading_angle = 0.25*M_PI;
vs.push(gv_y,heading_angle); // push the axis and angle vector
vs.axis_angle_to_quaternion(); // convert it to a quaternion
vs.quaternion_z_vector(); // pull the z vector out of the quaternion
vs.push(0.0,0.0,10.0,1.0); // arbitrary destination
vs.push(0.0,0.0,0.0,1.0); // my position
vs.subtract();
vs.normalize(); // get a normalized vector to my destination
float dot = vs.dot();
float angle_to_destination = acos(dot);
printf("angle_to_destination = %1.4f, heading_angle = %1.4f (should be the same)\n",
angle_to_destination,
heading_angle);
vs.push(gv_y,0.0);
vs.axis_angle_to_quaternion();
vs.quaternion_to_rotation_matrix();
vs.dump("quaternion_to_rotation_matrix() => (should look like 3x4 identity also)");
}
|
