/***************************************************************************** *    vector.c -  vector routines for quatlib that are unrelated to quaternions         (see quat.h for revision history and more documentation.)  * *****************************************************************************/#include "quat.h"/***************************************************************************** *   q_vec_print - prints a vector to stdout * *****************************************************************************/voidq_vec_print(q_vec_type vec){    printf("(%lf, %lf, %lf)\n", vec[Q_X], vec[Q_Y], vec[Q_Z]);}	/* q_vec_print *//***************************************************************************** *   q_set_vec - set vector equal to 3 values given * *****************************************************************************/voidq_set_vec(q_vec_type vec, double x, double y, double z){    vec[Q_X] = x;    vec[Q_Y] = y;    vec[Q_Z] = z;}	/* q_set_vec *//***************************************************************************** *   q_vec_copy - copies srcVec to destVec * *****************************************************************************/voidq_vec_copy(q_vec_type destVec, q_vec_type srcVec){    destVec[Q_X] = srcVec[Q_X];    destVec[Q_Y] = srcVec[Q_Y];    destVec[Q_Z] = srcVec[Q_Z];}	/* q_vec_copy */    /***************************************************************************** *   q_vec_add - adds two vectors     input:    	- destVec, aVec, bVec        output:    	- destVec = aVec + bVec        notes:    	- src and dest may be same storage * *****************************************************************************/voidq_vec_add(q_vec_type destVec, q_vec_type aVec, q_vec_type bVec){    int	    i;    for ( i = 0; i < 3; i++ )        destVec[i] = aVec[i] + bVec[i];}	/* q_vec_add *//***************************************************************************** *   q_vec_subtract - destVec = v1 - v2     input:    	- dest vector	- v1, v2        output:    	- dest vec        notes:    	- v1, v2, destVec need not be distinct storage * *****************************************************************************/voidq_vec_subtract(q_vec_type destVec, q_vec_type v1, q_vec_type v2){    destVec[Q_X] = v1[Q_X] - v2[Q_X];    destVec[Q_Y] = v1[Q_Y] - v2[Q_Y];    destVec[Q_Z] = v1[Q_Z] - v2[Q_Z];}	/* q_vec_subtract *//***************************************************************************** *   q_vec_dot_product - returns value of dot product of v1 and v2 * *****************************************************************************/doubleq_vec_dot_product(q_vec_type v1, q_vec_type v2){    return((v1[Q_X] * v2[Q_X]) + (v1[Q_Y] * v2[Q_Y]) + (v1[Q_Z] * v2[Q_Z]));}	/* q_vec_dot_product *//***************************************************************************** *   q_vec_scale - scale a vector     input:    	- pointer to destination vector	- scale factor	- pointer to src vector	    output:    	- dest vec is scaled by given amount        notes:    	- src and dest need not be distinct * *****************************************************************************/voidq_vec_scale(q_vec_type destVec, double scaleFactor, q_vec_type srcVec){    destVec[Q_X] = srcVec[Q_X] * scaleFactor;    destVec[Q_Y] = srcVec[Q_Y] * scaleFactor;    destVec[Q_Z] = srcVec[Q_Z] * scaleFactor;}	/* q_vec_scale *//***************************************************************************** *   q_vec_invert - negate a vector to point in the opposite direction     input:    	- pointer to destination vector	- pointer to src vector	    output:    	- dest vec is negated/inverted        notes:    	- src and dest need not be distinct	- this routine is called invert rather than negate to be consistent	  with q_invert for quaternions * *****************************************************************************/voidq_vec_invert(q_vec_type destVec, q_vec_type srcVec){    destVec[Q_X] = -srcVec[Q_X];    destVec[Q_Y] = -srcVec[Q_Y];    destVec[Q_Z] = -srcVec[Q_Z];}	/* q_vec_invert *//***************************************************************************** *   q_vec_magnitude - returns magnitude of vector     input:    	- vector        output:    	- magnitude     * *****************************************************************************/doubleq_vec_magnitude(q_vec_type vec){    return( sqrt( vec[Q_X]*vec[Q_X] + vec[Q_Y]*vec[Q_Y] + vec[Q_Z]*vec[Q_Z] ) );}	/* q_vec_magnitude *//***************************************************************************** *   q_vec_normalize - normalize a vector     input:    	vec - a vector	destVec - output parameter        output:    	normalized vector is put in destVec        notes:    	destVec and srcVec may be the same * *****************************************************************************/voidq_vec_normalize(q_vec_type destVec, q_vec_type srcVec){    double normalizeFactor;    double magnitude;    if ( (magnitude = q_vec_magnitude(srcVec)) < Q_EPSILON )    {        fprintf(stderr, "quatlib: q_vec_normalize: vector has 0 magnitude.\n");        return;    }    normalizeFactor = 1.0 / magnitude;    destVec[Q_X] = srcVec[Q_X] * normalizeFactor;    destVec[Q_Y] = srcVec[Q_Y] * normalizeFactor;    destVec[Q_Z] = srcVec[Q_Z] * normalizeFactor;}	/* q_vec_normalize *//***************************************************************************** *   q_vec_distance - returns distance between two points/vectors * *****************************************************************************/doubleq_vec_distance(q_vec_type vec1, q_vec_type vec2){    return( sqrt((vec2[Q_X] - vec1[Q_X]) * (vec2[Q_X] - vec1[Q_X]) + 	     (vec2[Q_Y] - vec1[Q_Y]) * (vec2[Q_Y] - vec1[Q_Y]) + 	     (vec2[Q_Z] - vec1[Q_Z]) * (vec2[Q_Z] - vec1[Q_Z])) );}	/* q_vec_distance *//***************************************************************************** *   q_vec_cross_product - computes cross product of two vectors:        	    	    	destVec = aVec X bVec     input:    	destVec:    pointer to output vector	aVec:  	    first vector	bVec:  	    second vector        output:    	destVec = aVec X bVec        notes:    	destVec == aVec or bVec ok. * *****************************************************************************/voidq_vec_cross_product(q_vec_type destVec, q_vec_type aVec, q_vec_type bVec){    q_vec_type	tmpDestVec;    tmpDestVec[Q_X] = aVec[Q_Y] * bVec[Q_Z] - aVec[Q_Z] * bVec[Q_Y];    tmpDestVec[Q_Y] = aVec[Q_Z] * bVec[Q_X] - aVec[Q_X] * bVec[Q_Z];    tmpDestVec[Q_Z] = aVec[Q_X] * bVec[Q_Y] - aVec[Q_Y] * bVec[Q_X];    q_vec_copy(destVec, tmpDestVec);}	/* q_vec_cross_product *//***************************************************************************** *   qp_vec_copy- copy PPHIGS srcVec to PPHIGS destVec * *****************************************************************************/void qp_vec_copy(Q_VectorType destVec, Q_VectorType srcVec){    destVec[X] = srcVec[X];    destVec[Y] = srcVec[Y];    destVec[Z] = srcVec[Z];}   /* qp_vec_copy  */    /***************************************************************************** *   qp_pvec_to_vec- convert PPHIGS srcVec to quatlib destVec * *****************************************************************************/void qp_pvec_to_vec(q_vec_type destVec, Q_VectorType srcVec){    destVec[Q_X] = srcVec[X];    destVec[Q_Y] = srcVec[Y];    destVec[Q_Z] = srcVec[Z];}   /* qp_pvec_to_vec  */    /***************************************************************************** *   qp_vec_to_pvec- convert quatlib srcVec to PPHIGS destVec * *****************************************************************************/void qp_vec_to_pvec(Q_VectorType destVec, q_vec_type srcVec){    destVec[X] = srcVec[Q_X];    destVec[Y] = srcVec[Q_Y];    destVec[Z] = srcVec[Q_Z];}   /* qp_vec_to_pvec  */