geometry.cpp

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

    return result;
}

Vector3 Transformer::transform_point(Vector3 const &p) {
    return current_transform->transform(p);
}

Vector3 Transformer::backtransform_point(Vector3 const &p) {
    const Matrix4 *const &t = current_transform;
    Vector3 mid;
    mid.x = p.x;
    mid.y = p.y;
    mid.z = p.z;

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

    mid.x -= row_0[3];
    mid.y -= row_1[3];
    mid.z -= row_2[3];

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

    return result;
}

void Transformer::transform_point(Vector3 const &p, Vector3 *result) {
    const Matrix4 *const &t = current_transform;

    const double *const row_0 = t->coef[0];
    const double *const row_1 = t->coef[1];
    const double *const row_2 = t->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];
}

Vector3 Transformer::transform_point_rotate_only(Vector3 const &p) {
    Matrix4 *t = current_transform;
    float x, y, z;

    x = p.x * t->coef[0][0] + p.y * t->coef[0][1] + p.z * t->coef[0][2];
    y = p.x * t->coef[1][0] + p.y * t->coef[1][1] + p.z * t->coef[1][2];
    z = p.x * t->coef[2][0] + p.y * t->coef[2][1] + p.z * t->coef[2][2];

    Vector3 result(x, y, z);

    return result;
}


void Matrix3::set(Quaternion q) {
    double Nq = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
    double s = (Nq > 0.0) ? (2.0 / Nq) : (0.0);

    double xs = q.x * s;
    double ys = q.y * s;
    double zs = q.z * s;

    double wx = q.w * xs;
    double wy = q.w * ys;
    double wz = q.w * zs;

    double xx = q.x * xs;
    double xy = q.x * ys;
    double xz = q.x * zs;

    double yy = q.y * ys;
    double yz = q.y * zs;
    double zz = q.z * zs;

    coef[0][0] = 1.0 - (yy + zz);
    coef[0][1] = xy - wz;
    coef[0][2] = xz + wy;

    coef[1][0] = xy + wz; 
    coef[1][1] = 1.0 - (xx + zz);
    coef[1][2] = yz - wx;

    coef[2][0] = xz - wy;
    coef[2][1] = yz + wx;
    coef[2][2] = 1.0 - (xx + yy);
}

void Matrix3::concat(Matrix3 *other) {
    int i, j, k;
    double accum;

    double new_coef[3][3];

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

			new_coef[i][j] = accum;
		}
    }

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

void Matrix3::identity() {
    coef[0][0] = 1.0;
    coef[0][1] = 0.0;
    coef[0][2] = 0.0;

    coef[1][0] = 0.0;
    coef[1][1] = 1.0;
    coef[1][2] = 0.0;

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

Vector3 lerp(const Vector3 &p0, const Vector3 &p1, double fraction) {
    Vector3 result;
    result.x = p0.x + fraction * (p1.x - p0.x);
    result.y = p0.y + fraction * (p1.y - p0.y);
    result.z = p0.z + fraction * (p1.z - p0.z);

    return result;
}

inline Quaternion lerp(const Quaternion &p0, const Quaternion &p1, double fraction) {
    Quaternion result;
    result.x = p0.x + fraction * (p1.x - p0.x);
    result.y = p0.y + fraction * (p1.y - p0.y);
    result.z = p0.z + fraction * (p1.z - p0.z);
    result.w = p0.w + fraction * (p1.w - p0.w);

    return result;
}

Quaternion slerp(Quaternion &start, Quaternion &end, double t) {
    // Input quaternions should be unit length or else
    // something broken will happen.

    // The technique below will work for input vectors of any
    // number of dimensions (you could write a templatized version
    // of the code, or one that takes a generic N-vector data type,
    // and it would just work).


    // Compute the cosine of the angle between the two vectors.
    double dot = dot_product(start, end);

    const double DOT_THRESHOLD = 0.9995;
    if (dot > DOT_THRESHOLD) {
        // If the inputs are too close for comfort, linearly interpolate
        // and normalize the result.
        Quaternion result = lerp(start, end, t);
        result.normalize();
        return result;
    }

    Clamp(dot, -1, 1);
    double theta_0 = acos(dot);  // Angle between input vectors
    double theta = theta_0 * t;  // Angle between 'start' and result

    Quaternion e1 = start;
    Quaternion e2 = end - start * dot;
    e2.normalize();              // { e1, e2 } is now an orthonormal basis

    return (e1 * cos(theta)) + (e2 * sin(theta));
}



inline double safe_acos(double x) {
    Clamp(x, -1, 1);
    return acos(x);
}

inline double safe_asin(double x) {
    Clamp(x, -1, 1);
    return asin(x);
}


void Quaternion::set_from_axis_and_angle(double _x, double _y, double _z,
										 double theta) {
    Vector3 vec(_x, _y, _z);
    vec.normalize();

    theta *= 0.5;
    double st = sin(theta);
    double ct = cos(theta);

    x = vec.x * st;
    y = vec.y * st;
    z = vec.z * st;
    w = ct;
}


Quaternion Quaternion::scale(float factor) {
    Quaternion result;
    result.x = x * factor;
    result.y = y * factor;
    result.z = z * factor;
    result.w = w * factor;

    return result;
}

void Quaternion::normalize() {
    double sum;

    sum = (double) w * (double) w + 
		(double) x * (double) x + 
		(double) y * (double) y + 
		(double) z * (double) z;

	assert(sum > 0);
	if (sum == 0) return;
	
    double ilen = 1.0 / sqrt(sum);

    w = w * ilen;
    x = x * ilen;
    y = y * ilen;
    z = z * ilen;
}

void Vector3::normalize() {
    double dx, dy, dz;

    dx = (double) x;	
    dy = (double) y;
    dz = (double) z;

    double sq = sqrt(dx * dx + dy * dy + dz * dz);
	assert(sq > 0);
    if (sq == 0) return;

	double factor = 1.0 / sq;
    x = dx * factor;
    y = dy * factor;
    z = dz * factor;
}

void Vector2::normalize() {
    double dx, dy;

    dx = (double) x;	
    dy = (double) y;

    double sq = sqrt(dx * dx + dy * dy);
	assert(sq > 0);
    if (sq == 0) return;

	double factor = 1.0 / sq;
    x = dx * factor;
    y = dy * factor;
}

void Vector3::safe_normalize() {
    double dx, dy, dz;

    dx = (double) x;	
    dy = (double) y;
    dz = (double) z;

    double sq = sqrt(dx * dx + dy * dy + dz * dz);
    if (sq == 0) return;  // @Numerical: Want some kind of epsilon here.

	double factor = 1.0 / sq;
    x = dx * factor;
    y = dy * factor;
    z = dz * factor;
}

float Vector3::length() {
    double dx,dy,dz;

    dx = (double) x;	
    dy = (double) y;
    dz = (double) z;

    float sq = (float)sqrt(dx * dx + dy * dy + dz * dz);
    return sq;
}

void Plane3::normal_set(Vector3 *p1, Vector3 *p2, Vector3 *p3){
    double v1_x,v1_y,v1_z;
    double v2_x,v2_y,v2_z;
    double nx,ny,nz;
    double len;

    v1_x = (double) p3->x - p2->x;
    v1_y = (double) p3->y - p2->y;
    v1_z = (double) p3->z - p2->z;

    v2_x = (double) p1->x - p2->x;
    v2_y = (double) p1->y - p2->y;
    v2_z = (double) p1->z - p2->z;

    nx = (v1_y * v2_z - v1_z * v2_y);
    ny = (v1_z * v2_x - v1_x * v2_z);
    nz = (v1_x * v2_y - v1_y * v2_x);

    len = sqrt((nx*nx) + (ny*ny) + (nz*nz));
  
    nx = nx/len;
    ny = ny/len;
    nz = nz/len;

    a = nx;
    b = ny;
    c = nz;
    d = -(a*p1->x + b*p1->y + c*p1->z);
}


void Plane3::normalize() {
    double da,db,dc,dd;

    da = (double) a;
    db = (double) b;
    dc = (double) c;
    dd = (double) d;

    double len = sqrt(da * da + db * db + dc * dc);
    double ilen = 1.0 / len;

    a = da * ilen;
    b = db * ilen;
    c = dc * ilen;
    d = dd * ilen;
}

double distance(const Vector3 &p1, const Vector3 &p2) {
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    double dz = p2.z - p1.z;

    return sqrt(dx * dx + dy * dy + dz * dz);
}

float arctan2(float y, float x) {
    if (x == 0.0) {
        return (y < 0) ? (-M_PI/2) : (M_PI/2);
    }

    return (float)atan2(y, x);
}



void Matrix4::copy_33(Matrix3 *rm) {
    int j, i;
    for (j = 0; j < 3; j++) {
        for (i = 0; i < 3; i++) {
			coef[i][j] = rm->coef[i][j];
		}
    }
}

void orthonormalize(Vector3 *v, Vector3 *w) {
    v->normalize();
    double dot = dot_product(*v, *w);

    Vector3 proj = *v;
    proj.scale(dot);
    *w -= proj;

    w->normalize();
}



float Matrix3::invert() {
    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];

    double det3;
    det3 = a * (e*i - f*h) + d * (h*c - b*i) + g * (b*f - c*e);
    
    if (det3 == 0) return 0;
    assert(det3 != 0.0);

    double factor = 1.0 / det3;

    coef[0][0] =  (e * i - f * h) * factor;
    coef[1][0] = -(d * i - f * g) * factor;
    coef[2][0] =  (d * h - e * g) * factor;
    coef[0][1] = -(b * i - c * h) * factor;
    coef[1][1] =  (a * i - c * g) * factor;
    coef[2][1] = -(a * h - b * g) * factor;
    coef[0][2] =  (b * f - c * e) * factor;
    coef[1][2] = -(a * f - c * d) * factor;
    coef[2][2] =  (a * e - b * d) * factor;

    return det3;
}

void Matrix3::transpose() {
    double b = coef[0][1];
    double c = coef[0][2];
    double d = coef[1][0];
    double f = coef[1][2];
    double g = coef[2][0];
    double h = coef[2][1];

    coef[1][0] = b;
    coef[2][0] = c;
    coef[0][1] = d;
    coef[2][1] = f;
    coef[0][2] = g;
    coef[1][2] = h;
}