The result is a vector stack, inspired by Forth. The most basic operations consist of pushing vectors onto the stack and performing operations on them that implicitly use the stack as source and destination. For example:
vector_stack.push(2.0, 1.0, 4.0, 0.0);
vector_stack.normalize();
Here I push a vector (2,1,4,0) on the stack, then call normalize which implicity normalizes the top of the stack, hence normalizes my vector. I can then treat it like a vector in space and go from one point to another:
vector_stack.push(10.0, 0.0, 10.0, 1.0);
vector_stack.add();
Now the stack contains only my resulting point. There are more advanced operations in the code below.
#include
#include
using namespace std;
extern void test_vector_stack();
int main(int argc, char *argv[])
{
test_vector_stack();
system("PAUSE");
return EXIT_SUCCESS;
}
#include
#include
#include
#include
//
// 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)");
