kludge3d-20040822/src/vector.c

/*
 * File:         vector.c
 *
 * Description:  funcs for doing vector math
 *
 * This source code is part of kludge3d, and is released under the
 * GNU General Public License.
 *
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "vector.h"

#ifdef MEMWATCH
#include "memwatch.h"
#endif


/* clear v1
 */
void vector_zero( float *v1 ) {

    int i;

    if( v1 == NULL ) return;

    for( i = 0; i < 3; i++ ) {
        v1[i] = 0.0;
    }

}

/* copies contents of v2 to v1
 * Note that it's ok for v1 to actually point to v2
 * (it wouldn't make sense to do so, but...)
 */
void vector_copy( float *v1, float *v2 ) {

    int i;

    if( v1 == NULL || v2 == NULL ) return;

    for( i = 0; i < 3; i++ ) {
        v1[i] = v2[i];
    }

}

/* adds v3 to v2, stores result in v1
 * Note that it's ok for v1 to actually point to v2 or v3
 */
void vector_add( float *v1, float *v2, float *v3 ) {

    int i;

    if( v1 == NULL || v2 == NULL || v3 == NULL ) return;

    for( i = 0; i < 3; i++ ) {
        v1[i] = v2[i] + v3[i];
    }

}

/* subtracts v3 from v2, stores result in v1
 * Note that it's ok for v1 to actually point to v2 or v3
 */
void vector_sub( float *v1, float *v2, float *v3 ) {

    int i;

    if( v1 == NULL || v2 == NULL || v3 == NULL ) return;

    for( i = 0; i < 3; i++ ) {
        v1[i] = v2[i] - v3[i];
    }

}


/* finds the length of v1 (ie |v1|) */
float vector_length( float *v1 ) {

    return sqrt ( ( v1[0] * v1[0] ) + ( v1[1] * v1[1] ) + ( v1[2] * v1[2] ) );
}


/* finds the length^2 of v1 (ie |v1|^2) */
float vector_length_sqrd( float *v1 ) {

    return ( v1[0] * v1[0] ) + ( v1[1] * v1[1] ) + ( v1[2] * v1[2] );
}


/* makes length( v1 ) equal to 1.0, also returns length before normalization */
float vector_normalize( float *v1 ) {

    int i;
    float mag1;

    if( v1 == NULL ) return -1.0;

    // Compute magnitude
    mag1 = vector_length( v1 );

    // Normalize
    for( i = 0; i < 3; i++ ) {
        v1[i] = ( v1[i] / mag1 );
    }

	return mag1;
}


/* finds cross product of v2 and v3, stores result in v1
 * Note that you should never use v2 or v3 as v1!
 */
void vector_cross_prod( float *v1, float *v2, float *v3 ) {

    if( v1 == NULL || v2 == NULL || v3 == NULL ) return;

    // Compute Cross Product
    v1[0] = ( v2[1] * v3[2] ) - ( v2[2] * v3[1] );
    v1[1] = ( v2[2] * v3[0] ) - ( v2[0] * v3[2] );
    v1[2] = ( v2[0] * v3[1] ) - ( v2[1] * v3[0] );

}


/* returns dot product (a scalar) of v1 and v2 */
float vector_dot_prod( float *v1, float *v2 ) {

    if( v1 == NULL || v2 == NULL ) return 0.0f;

    return
    ( v1[0] * v2[0] ) +
    ( v1[1] * v2[1] ) +
    ( v1[2] * v2[2] );

}


/* multiplies elements of v2 by s (a scalar) and stores results in v1
 * Note that it's ok for v1 to actually point to v2
 */
void vector_mul( float *v1, float *v2, float s ) {

    if( v1 == NULL || v2 == NULL ) return;

    // multiply vector by scalar
    v1[0] = v2[0] * s;
    v1[1] = v2[1] * s;
    v1[2] = v2[2] * s;

}


/* multiplies elements of v2 by corresponding element in v3, and stores
 * results in v1
 * Note that it's ok for v1 to actually point to v2 or v3
 */
void vector_mul_piecewise( float *v1, float *v2, float *v3 ) {

    if( v1 == NULL || v2 == NULL || v3 == NULL ) return;

    v1[0] = v2[0] * v3[0];
    v1[1] = v2[1] * v3[1];
    v1[2] = v2[2] * v3[2];
}


/* treats v2 as the origin of a ray, v3 as its direction, and finds the
 * point v1 at distance s from the ray's origin
 * Note - it is NOT ok for v1 and v2 to point to the same vector!
 */
void vector_find_point( float *v1, float *v2, float *v3, float s ) {

    if( v1 == NULL || v2 == NULL || v3 == NULL ) return;

	vector_mul( v1, v3, s );
	vector_add( v1, v1, v2 );

}


float vector_find_dist_pt2line( float *rayorig, float *raydir, float *pt ) {

	float t0, result;
	float delta[3];
	float temp[3];

    if( rayorig == NULL || raydir == NULL || pt == NULL ) return 0.0;

	vector_sub( delta, pt, rayorig );
	t0 = vector_dot_prod( raydir, delta ) / vector_dot_prod( raydir, raydir );

	if( t0 > 0.0 ) {
		vector_find_point( temp, rayorig, raydir, t0 );
		vector_sub( temp, pt, temp );
		result = vector_length( temp );
	} else {
		result = vector_length( delta );
	}

	return result;
}


/* v is assumed to be normalized
   the z component for v is assumed to be 0
   returns heading in radians
*/
float vector_find_heading( float *v ) {
	float xAxis[] = { 1.0, 0.0, 0.0 };
	float deltaV2V1[3], invV[3], tempPt[3];
	int reflexAngle = 0;
	float cosTheta, result = 0.0;

	if( v == NULL ) return 0.0;

	/* This is a tricky bit of code, and it took me forever to work this out.
	   Dot product gets us the cosine of the angle between two unit vectors,
	   but its the SMALLEST angle between the two.  This is no good for
	   "reflex angles," ie angles greater then 180.  The following code
	   discriminates between reflex and non-reflex angles.  In short, if the
	   normal of v1 and deltaV2V1 points up, all is well.  If it points down,
	   that means that we have a reflex angle, and need to take appropriate
	   action.
	 */
	vector_sub( deltaV2V1, xAxis, v );
	vector_normalize( deltaV2V1 );
	/* flip v1 around */
	vector_mul( invV, v, -1.0 );
	vector_cross_prod( tempPt, invV, deltaV2V1 );
	if( tempPt[2] < 0.0 ) {
		reflexAngle = 1;
	}

	cosTheta = vector_dot_prod( v, xAxis );
	result = acos( cosTheta );
	if( reflexAngle )
		result = (2*M_PI) - result;

/*printf( "v: < %3.3f %3.3f %3.3f > costheta: %3.3f result: %3.3f reflang?: %i\n",
v[0],
v[1],
v[2],
cosTheta,
result,
reflexAngle );*/

	return result;
}


float vector_dist( float *v1, float *v2 ) {
	float temp[3];

    if( v1 == NULL || v2 == NULL ) return 0.0f;

	vector_sub( temp, v2, v1 );
	return vector_length( temp );
}


float vector_dist_sqrd( float *v1, float *v2 ) {
	float temp[3];

    if( v1 == NULL || v2 == NULL ) return 0.0f;

	vector_sub( temp, v2, v1 );
	return vector_length_sqrd( temp );
}


/* is angle between vectors < 90 degrees? args must be normalized. */
int vector_coincident( float *v1, float *v2 ) {

    if( v1 == NULL || v2 == NULL ) return 0;

	if( vector_dot_prod( v1, v2 ) > 0.0 ) {
		/* The dot prod of two unit vectors gets us the cosine of the
		   angle between them.  At this point, since the cosine is positive,
		   we know that the angle is from either 0-90 or 270-360.  Since
		   the dotprod trick will always measure the shortest angle between
		   the vectors, we know that the angles are in fact coincidental.
		   (now that I think about it, 270-360 works too :)
		*/
		return 1;
	}
	return 0;
}


void vector_rotate_about_point(
	float *result, float *v, int axis, float *origin, float angle )
{
	/* Note - don't use this func if you are rotating a whole bunch of
	objects by the same amount.  A func for doing so (for Verts, at least)
	already exists.  (cos and sin are expensive funcs, and need only be
	executed once per rotation, not once per object) */

	float cosine = cos(angle);
	float sine = sin(angle);

	if( result == NULL )
		return;
	if( v == NULL )
		return;
	if( origin == NULL )
		return;

	vector_copy( result, v );

	{

	float temp0, temp1, temp2;

	if( axis == 0 ) {
		temp1 = v[1] - origin[1];
		temp2 = v[2] - origin[2];

		result[1] = temp1*cosine - temp2*sine + origin[1];
		result[2] = temp1*sine + temp2*cosine + origin[2];
	} else if( axis == 1 ) {
		temp0 = v[0] - origin[0];
		temp2 = v[2] - origin[2];

		result[0] =   temp0*cosine + temp2*sine + origin[0];
		result[2] = - temp0*sine + temp2*cosine + origin[2];
	} else if( axis == 2 ) {
		temp0 = v[0] - origin[0];
		temp1 = v[1] - origin[1];

		result[0] = temp0*cosine - temp1*sine + origin[0];
		result[1] = temp0*sine + temp1*cosine + origin[1];
	}

	}

}