geometry.cpp

国外游戏开发者杂志2003年第七期配套代码

#include "../framework.h"
#include <math.h>

void Matrix4::zero_matrix() {
    int i, j;

    for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			coef[i][j] = 0;
		}
    }
}

void Matrix4::identity() {
    zero_matrix();

    coef[0][0] = 1.0;
    coef[1][1] = 1.0;
    coef[2][2] = 1.0;
    coef[3][3] = 1.0;
}

void Matrix4::scale_coefficients(double factor) {
    int i, j;
    for (j = 0; j < 3; j++) {
        for (i = 0; i < 3; i++) {
			coef[j][i] *= factor;
		}
    }
}

void Matrix4::set_rotation(const Quaternion &q) {
    Matrix3 rm;
    rm.set(q);

    int i, j;
    for (j = 0; j < 3; j++) {
        for (i = 0; i < 3; i++) {
			coef[j][i] = rm.coef[j][i];
		}
    }
}

void Matrix4::invert_33() {
    double adj[3][3];
    double a = coef[0][0];
    double b = coef[0][1];
    double c = coef[0][2];
    double d = coef[1][0];
    double e = coef[1][1];
    double f = coef[1][2];
    double g = coef[2][0];
    double h = coef[2][1];
    double i = coef[2][2];

    adj[0][0] = e * i - f * h;
    adj[1][0] = -(d * i - f * g);
    adj[2][0] = d * h - e * g;
    adj[0][1] = -(b * i - c * h);
    adj[1][1] = a * i - c * g;
    adj[2][1] = -(a * h - b * g);
    adj[0][2] = b * f - c * e;
    adj[1][2] = -(a * f - c * d);
    adj[2][2] = a * e - b * d;
    
    double det3;
    
    double a11 = a; double a12 = b; double a13 = c;
    double a21 = d; double a22 = e; double a23 = f;
    double a31 = g; double a32 = h; double a33 = i;

    det3 = a * (a22*a33-a23*a32)
		+ d * (a32*a13-a12*a33)
		+ g * (a12*a23-a22*a13);


    double scale = 1.0f / det3;

    int x, y;
    for (x = 0; x < 3; x++) {
		for (y = 0; y < 3; y++) {
			coef[y][x] = adj[y][x] * scale;
		}
    }
}

// @Incomplete: put a full 4x4 matrix inversion here
void Matrix4::invert() {
	// This is not a full 4x4 invert... no projection allowed.
	assert(coef[3][0] == 0);
	assert(coef[3][1] == 0);
	assert(coef[3][2] == 0);
	assert(coef[3][3] == 1);

	Vector3 t(coef[0][3], coef[1][3], coef[2][3]);
	invert_33();

	coef[0][3] = -(t.x * coef[0][0] + t.y * coef[0][1] + t.z * coef[0][2]);
	coef[1][3] = -(t.x * coef[1][0] + t.y * coef[1][1] + t.z * coef[1][2]);
	coef[2][3] = -(t.x * coef[2][0] + t.y * coef[2][1] + t.z * coef[2][2]);
}

void Matrix4::rotate(const Quaternion &q) {
    Matrix4 rot_matrix;
    rot_matrix.identity();
    rot_matrix.set_rotation(q);

    Matrix4 result;
    result.next = next;

    transform_multiply(&rot_matrix, this, &result);
    memcpy(this, &result, sizeof(Matrix4));
}

void Matrix4::translate(const Vector3 &t) {
    coef[0][3] += t.x;
    coef[1][3] += t.y;
    coef[2][3] += t.z;
}

void Matrix4::translate(double x, double y, double z) {
    coef[0][3] += x;
    coef[1][3] += y;
    coef[2][3] += z;
}

void Matrix4::scale(const Vector3 &v) {
    coef[0][0] *= v.x;
    coef[0][1] *= v.x;
    coef[0][2] *= v.x;
    coef[0][3] *= v.x;

    coef[1][0] *= v.y;
    coef[1][1] *= v.y;
    coef[1][2] *= v.y;
    coef[1][3] *= v.y;

    coef[2][0] *= v.z;
    coef[2][1] *= v.z;
    coef[2][2] *= v.z;
    coef[2][3] *= v.z;
}

void Matrix4::shear(const Vector3 &v) {
    Matrix4 result;
    result.next = next;

    Matrix4 shear_matrix;
    shear_matrix.identity();
    shear_matrix.coef[1][0] = v.x;
    shear_matrix.coef[2][0] = v.y;
    shear_matrix.coef[2][1] = v.z;

    transform_multiply(&shear_matrix, this, &result);
    memcpy(this, &result, sizeof(Matrix4));
}


void transform_multiply(Matrix4*x, Matrix4*y, Matrix4*r) {
    int i, j, k;
    double accum;

    for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			accum = 0;
			for (k = 0; k < 4; k++) {
				accum += x->coef[i][k] * y->coef[k][j];
			}

			r->coef[i][j] = accum;
		}
    }
}

// Cut-and-pasted from the above.
void rotation_multiply(Matrix3 *x, Matrix3 *y,
                       Matrix3 *r) {
    int i, j, k;
    double accum;

    for (i = 0; i < 3; i++) {
		for (j = 0; j < 3; j++) {
			accum = 0;
			for (k = 0; k < 3; k++) {
				accum += x->coef[i][k] * y->coef[k][j];
			}

			r->coef[i][j] = accum;
		}
    }
}

void Vector3::rotate(const Quaternion &q) {
    Matrix3 rm;
    rm.set(q);
    rotate(&rm);
}



inline Matrix4 *create_transformation_matrix(Transformer *tr) {
    Matrix4 *tm = new Matrix4;
    return tm;
}

inline void destroy_Matrix4(Transformer *tr,
										  Matrix4 *tm) {
    delete tm;
}

Transformer::Transformer() {
    current_transform = create_transformation_matrix(this);
    current_transform->identity();
    current_transform->next = NULL;
};

Transformer::~Transformer() {
    Matrix4 *m;
};

void Transformer::push_identity() {
    Matrix4 *t = create_transformation_matrix(this);
    t->identity();
    t->next = current_transform;
    current_transform = t;
}

void Transformer::push(Matrix4 *q) {
    Matrix4 *t = create_transformation_matrix(this);

    transform_multiply(current_transform, q, t);
    t->next = current_transform;
    current_transform = t;
}

void Transformer::push(Matrix3 *rmatrix, 
					   Vector3 about, Vector3 translation) {
    Matrix4 t1, t2, r1, tmp, result;

    t1.identity();
    t1.translate(-about.x, -about.y, -about.z);

    r1.identity();
    int i, j;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
			r1.coef[i][j] = rmatrix->coef[i][j];
		}
    }

    transform_multiply(&r1, &t1, &tmp);

    t2.identity();
    t2.translate(about.x + translation.x,
				 about.y + translation.y,
				 about.z + translation.z);

    transform_multiply(&t2, &tmp, &result);

    push(&result);
}

void Transformer::push(const Quaternion &ori, const Vector3 &pos) {
    Matrix3 rm;
    rm.set(ori);

    push(&rm, Vector3(0, 0, 0), Vector3(pos.x, pos.y, pos.z));
}

void Transformer::push_with_scale(Matrix3 *rmatrix, 
								  Vector3 about, Vector3 translation,
								  Vector3 scale) {
    Matrix4 t1, t2, r1, tmp, result;

    t1.identity();
    t1.translate(-about.x, -about.y, -about.z);

    float scale_array[3];
    scale_array[0] = scale.x;
    scale_array[1] = scale.y;
    scale_array[2] = scale.z;

    r1.identity();
    int i, j;
    for (i = 0; i < 3; i++) {
        for (j = 0; j < 3; j++) {
			r1.coef[i][j] = rmatrix->coef[i][j] * scale_array[i];
		}
    }

    transform_multiply(&r1, &t1, &tmp);

    t2.identity();
    t2.translate(about.x + translation.x,
				 about.y + translation.y,
				 about.z + translation.z);

    transform_multiply(&t2, &tmp, &result);

    push(&result);
}

void Transformer::pop() {
    assert(current_transform->next != NULL);

    Matrix4 *t = current_transform;
    current_transform = (Matrix4 *)t->next;
    destroy_Matrix4(this, t);
}

void Transformer::pop(Matrix4 *dest) {
    assert(current_transform->next != NULL);

    Matrix4 *t = current_transform;
    current_transform = (Matrix4 *)t->next;

    memcpy(dest, t, sizeof(Matrix4));
    dest->next = NULL;

    destroy_Matrix4(this, t);
}

void Matrix4::get_translation(Vector3 *result) {
    result->x = (coef[0])[3];
    result->y = (coef[1])[3];
    result->z = (coef[2])[3];
}

void Matrix4::remove_translation(Vector3 *result) {
    get_translation(result);
    coef[0][3] = 0;
    coef[1][3] = 0;
    coef[2][3] = 0;
}

void Matrix4::copy_from(Matrix4 *other) {
    memcpy(this, other, sizeof(Matrix4));
}

#define caseMacro(i, j, k, I, J, K) \
  case I:\
    s = sqrt((coef[I][I] - (coef[J][J] + coef[K][K])) + coef[3][3]); \
    q.i = s*0.5;\
    s = 0.5 / s;\
    q.j = (coef[I][J] + coef[J][I]) * s;\
    q.k = (coef[K][I] + coef[I][K]) * s;\
    q.w = (coef[K][J] - coef[J][K]) * s;\
    break;


void Matrix4::get_orientation(Quaternion *result) {
    double trace = coef[0][0] + coef[1][1] + coef[2][2];
    double s;
    Quaternion q;

    if (trace >= 0.0) {
        s = sqrt(trace + coef[3][3]);
		q.w = s*0.5;
		s = 0.5 / s;
		q.x = (coef[2][1] - coef[1][2]) * s;
		q.y = (coef[0][2] - coef[2][0]) * s;
		q.z = (coef[1][0] - coef[0][1]) * s;
    } else {
        int h = 0;
		if (coef[1][1] > coef[h][h]) h = 1;
		if (coef[2][2] > coef[h][h]) h = 2;

		switch (h) {
			caseMacro(x, y, z, 0, 1, 2);
			caseMacro(y, z, x, 1, 2, 0);
			caseMacro(z, x, y, 2, 0, 1);
		}
    }

    assert(coef[3][3] == 1.0);
    if (coef[3][3] != 1.0) q = q.scale(1.0 / sqrt(coef[3][3]));

    q.normalize();
    *result = q;
}

// Cut-and-pasted from Matrix4::get_orientation
void Matrix3::get_orientation(Quaternion *result) {
    double trace = coef[0][0] + coef[1][1] + coef[2][2];
    double s;
    Quaternion q;

    if (trace >= 0.0) {
        s = sqrt(trace);
		q.w = s*0.5;
		s = 0.5 / s;
		q.x = (coef[2][1] - coef[1][2]) * s;
		q.y = (coef[0][2] - coef[2][0]) * s;
		q.z = (coef[1][0] - coef[0][1]) * s;
    } else {
        int h = 0;
		if (coef[1][1] > coef[h][h]) h = 1;
		if (coef[2][2] > coef[h][h]) h = 2;

		switch (h) {
			caseMacro(x, y, z, 0, 1, 2);
			caseMacro(y, z, x, 1, 2, 0);
			caseMacro(z, x, y, 2, 0, 1);
		}
    }

    q.normalize();
    *result = q;
}

Vector3 Matrix4::transform(Vector3 const &p) {
    Vector3 result;

    const double *const row_0 = coef[0];
    const double *const row_1 = coef[1];
    const double *const row_2 = coef[2];

    result.x = p.x * row_0[0] + p.y * row_0[1] + p.z * row_0[2]
		+ row_0[3];

    result.y = p.x * row_1[0] + p.y * row_1[1] + p.z * row_1[2]
		+ row_1[3];

    result.z = p.x * row_2[0] + p.y * row_2[1] + p.z * row_2[2]
		+ row_2[3];

    return result;
}

Vector3 Matrix4::transform_rotate_only(Vector3 const &p) {
    Vector3 result;

    const double *const row_0 = coef[0];
    const double *const row_1 = coef[1];
    const double *const row_2 = coef[2];

    result.x = p.x * row_0[0] + p.y * row_0[1] + p.z * row_0[2];
    result.y = p.x * row_1[0] + p.y * row_1[1] + p.z * row_1[2];
    result.z = p.x * row_2[0] + p.y * row_2[1] + p.z * row_2[2];