www.pudn.com > HumanAnimation.rar > VECTOR.CPP

#include "stdafx.h" 
#include  
#include  
#include "vector.h" 
#include "body.h" 
 
void vecPrint (vector v) 
{ 
  printf ("[%f %f %f]\n", v[0], v[1], v[2]); 
} 
 
void vecCopy (vector dest, vector src) 
{ 
  dest[0] = src[0]; 
  dest[1] = src[1]; 
  dest[2] = src[2]; 
} 
 
float vecLength (vector u) 
{ 
  return sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); 
} 
 
float vecLengthSq (vector u) 
{ 
  return (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); 
} 
 
float vecDist (vector   u, 
	 vector   v) 
{ 
  return sqrt ((v[0] - u[0]) * (v[0] - u[0]) + 
	       (v[1] - u[1]) * (v[1] - u[1]) + 
	       (v[2] - u[2]) * (v[2] - u[2])); 
} 
 
float vecDistSq (vector   u, 
	   vector   v) 
{ 
  return ((v[0] - u[0]) * (v[0] - u[0]) + 
	  (v[1] - u[1]) * (v[1] - u[1]) + 
	  (v[2] - u[2]) * (v[2] - u[2])); 
} 
 
void vecSub (vector     dest,  
	vector     u, 
	vector     v) 
{ 
  vector    res; 
 
  res[0] = u[0] - v[0]; 
  res[1] = u[1] - v[1]; 
  res[2] = u[2] - v[2]; 
 
  vecCopy (dest, res); 
} 
 
void vecAdd (vector     dest,  
	vector     u, 
	vector     v) 
{ 
  vector     res; 
 
  res[0] = u[0] + v[0]; 
  res[1] = u[1] + v[1]; 
  res[2] = u[2] + v[2]; 
 
  vecCopy (dest, res); 
} 
 
void vecMult (vector     dest,  
	 float      c, 
	 vector     u) 
{ 
  vector    res; 
 
  res[0] = u[0] * c; 
  res[1] = u[1] * c; 
  res[2] = u[2] * c; 
 
  vecCopy (dest, res); 
} 
 
float vecDot (vector    u, 
	vector    v) 
{ 
  return (u[0] * v[0] + u[1] * v[1] + u[2] * v[2]); 
} 
 
float vecAngle (vector   u, 
	  vector   v) 
{ 
  return acos (vecDot (u, v) / (vecLength (u) * vecLength (v))); 
} 
 
void vecCross (vector    dest, 
	  vector    u, 
	  vector    v) 
{ 
  vector res; 
 
  res[0] = u[1] * v[2] - u[2] * v[1]; 
  res[1] = u[2] * v[0] - u[0] * v[2]; 
  res[2] = u[0] * v[1] - u[1] * v[0]; 
 
  vecCopy (dest, res); 
} 
 
void vecNormalize (vector dest, vector v) 
{ 
  float   length; 
 
  length = vecLength (v); 
   
  if (length != 0) 
    { 
      vecMult (dest, 1 / length, v); 
    } 
} 
 
float vecDistPointToPlane (vector    r, 
		     plane     p) 
{ 
  return (p[0] * r[0] + p[1] * r[1] + p[2] * r[2] + p[3]); 
} 
 
void vecPlaneFromPointAndNormal (plane    dest, 
			    vector   p, 
			    vector   N) 
{ 
  dest[0] = N[0]; 
  dest[1] = N[1]; 
  dest[2] = N[2]; 
  dest[3] = -((N[0] * p[0]) + (N[1] * p[1]) + (N[2] * p[2])); 
} 
 
void vecSpherical (vector dest, float theta, float phi) 
{ 
  dest[0] = (float)sin (theta) * (float)cos (phi); 
  dest[1] = (float)sin (theta) * (float)sin (phi); 
  dest[2] = (float)cos (theta); 
} 
 
void vecRotateX (vector     dest, 
	    vector     src, 
	    float      angle) 
{ 
  float    s; 
  float    c; 
  vector   result; 
 
  s = (float)sin (angle * VEC_DEG_TO_RAD); 
  c = (float)cos (angle * VEC_DEG_TO_RAD); 
 
  result[0] = src[0]; 
  result[1] = src[1] * c + src[2] * s; 
  result[2] = -src[1] * s + src[2] * c; 
 
  vecCopy (dest, result); 
} 
 
void vecRotateY (vector      dest, 
	    vector      src, 
	    float       angle) 
{ 
  float    s; 
  float    c; 
  vector   result; 
 
  s = (float)sin (angle * VEC_DEG_TO_RAD); 
  c = (float)cos (angle * VEC_DEG_TO_RAD); 
 
  result[0] = src[0] * c - src[2] * s; 
  result[1] = src[1]; 
  result[2] = src[0] * s + src[2] * c; 
 
  vecCopy (dest, result); 
} 
 
void vecRotateZ (vector     dest, 
	    vector     src, 
	    float      angle) 
{ 
  float   s; 
  float   c; 
  vector  result; 
 
  s = (float)sin (angle * VEC_DEG_TO_RAD); 
  c = (float)cos (angle * VEC_DEG_TO_RAD); 
 
  result[0] = src[0] * c + src[1] * s; 
  result[1] = -src[0] * s + src[1] * c; 
  result[2] = src[2]; 
   
  vecCopy (dest, result); 
} 
 
void vecRotate (vector dest, vector p, vector axis, float angle) 
{ 
  float   c; 
  float   s; 
  float   t; 
  float   x, y, z; 
  float   x2, y2, z2; 
  float   R[3][3]; 
  vector  result; 
 
  c = (float)cos (angle); 
  s = (float)sin (angle); 
  t = 1 - c; 
  x = axis[0]; 
  y = axis[1]; 
  z = axis[2]; 
  x2 = x * x; 
  y2 = y * y; 
  z2 = z * z; 
 
  R[0][0] = (t * x2) + c; 
  R[1][0] = (t * x * y) - (s * z); 
  R[2][0] = (t * x * z) + (s * y); 
   
  R[0][1] = (t * x * y) + (s * z); 
  R[1][1] = (t * y2) + c; 
  R[2][1] = (t * y * z) - (s * x); 
 
  R[0][2] = (t * x * z) - (s * y); 
  R[1][2] = (t * y * z) + (s * x); 
  R[2][2] = (t * z2) + c; 
 
  result[0] = (p[0] * R[0][0]) + (p[1] * R[1][0]) + (p[2] * R[2][0]); 
  result[1] = (p[0] * R[0][1]) + (p[1] * R[1][1]) + (p[2] * R[2][1]); 
  result[2] = (p[0] * R[0][2]) + (p[1] * R[1][2]) + (p[2] * R[2][2]); 
   
  vecCopy (dest, result); 
} 
 
void vecMatch (float *angle, vector axis, vector u, vector v) 
{ 
  vecCross (axis, u, v); 
  vecNormalize (axis, axis); 
  *angle = acos (vecDot (u, v)); 
} 
 
void vecSetValue(vector dest, float x, float y, float z) 
{ 
  vector  result; 
 
  result[X] = x; 
  result[Y] = y; 
  result[Z] = z; 
 
  vecCopy(dest, result); 
} 
 
void vecTranslate(vector dest, float x, float y, float z) 
{ 
  vector  result; 
  result[X] = dest[X] + x; 
  result[Y] = dest[Y] + y; 
  result[Z] = dest[Z] + z; 
 
  vecCopy(dest, result); 
}