matvec.h

PQP is a library for performing three types of proximity queries on a pair of geometric models compo

/*************************************************************************\

  Copyright 1999 The University of North Carolina at Chapel Hill.
  All Rights Reserved.

  Permission to use, copy, modify and distribute this software and its
  documentation for educational, research and non-profit purposes, without
  fee, and without a written agreement is hereby granted, provided that the
  above copyright notice and the following three paragraphs appear in all
  copies.

  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
  DAMAGES.

  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE
  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.

  The authors may be contacted via:

  US Mail:             S. Gottschalk
                       Department of Computer Science
                       Sitterson Hall, CB #3175
                       University of N. Carolina
                       Chapel Hill, NC 27599-3175

  Phone:               [phone]

  EMail:               [e-mail]


\**************************************************************************/

#ifndef PQP_MATVEC_H
#define PQP_MATVEC_H

#include <math.h>
#include <stdio.h>
#include "PQP_Compile.h"

#ifndef M_PI
const double M_PI =  3.14159265359;
#endif

#ifdef gnu
#include "zzzz.h"

#ifdef hppa
#define myfabs(x) \
 ({double __value, __arg = (x); \
   asm("fabs,dbl %1, %0": "=f" (__value): "f" (__arg)); \
   __value; \
});
#endif

#ifdef mips
#define myfabs(x) \
 ({double __value, __arg = (x); \
   asm("abs.d %0, %1": "=f" (__value): "f" (__arg)); \
   __value; \
});
#endif

#else  

#define myfabs(x) ((x < 0) ? -x : x)

#endif


inline
void
Mprintg(const PQP_REAL M[3][3])
{
  printf("%g %g %g\n%g %g %g\n%g %g %g\n",
	 M[0][0], M[0][1], M[0][2],
	 M[1][0], M[1][1], M[1][2],
	 M[2][0], M[2][1], M[2][2]);
}


inline
void
Mfprint(FILE *f, const PQP_REAL M[3][3])
{
  fprintf(f, "%g %g %g\n%g %g %g\n%g %g %g\n",
	 M[0][0], M[0][1], M[0][2],
	 M[1][0], M[1][1], M[1][2],
	 M[2][0], M[2][1], M[2][2]);
}

inline
void
Mprint(const PQP_REAL M[3][3])
{
  printf("%g %g %g\n%g %g %g\n%g %g %g\n",
	 M[0][0], M[0][1], M[0][2],
	 M[1][0], M[1][1], M[1][2],
	 M[2][0], M[2][1], M[2][2]);
}

inline
void
Vprintg(const PQP_REAL V[3])
{
  printf("%g %g %g\n", V[0], V[1], V[2]);
}

inline
void
Vfprint(FILE *f, const PQP_REAL V[3])
{
  fprintf(f, "%g %g %g\n", V[0], V[1], V[2]);
}

inline
void
Vprint(const PQP_REAL V[3])
{
  printf("%g %g %g\n", V[0], V[1], V[2]);
}

inline
void
Midentity(PQP_REAL M[3][3])
{
  M[0][0] = M[1][1] = M[2][2] = 1.0;
  M[0][1] = M[1][2] = M[2][0] = 0.0;
  M[0][2] = M[1][0] = M[2][1] = 0.0;
}

inline
void
Videntity(PQP_REAL T[3])
{
  T[0] = T[1] = T[2] = 0.0;
}

inline
void
McM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
{
  Mr[0][0] = M[0][0];  Mr[0][1] = M[0][1];  Mr[0][2] = M[0][2];
  Mr[1][0] = M[1][0];  Mr[1][1] = M[1][1];  Mr[1][2] = M[1][2];
  Mr[2][0] = M[2][0];  Mr[2][1] = M[2][1];  Mr[2][2] = M[2][2];
}

inline
void
MTcM(PQP_REAL Mr[3][3], const PQP_REAL M[3][3])
{
  Mr[0][0] = M[0][0];  Mr[1][0] = M[0][1];  Mr[2][0] = M[0][2];
  Mr[0][1] = M[1][0];  Mr[1][1] = M[1][1];  Mr[2][1] = M[1][2];
  Mr[0][2] = M[2][0];  Mr[1][2] = M[2][1];  Mr[2][2] = M[2][2];
}

inline
void
VcV(PQP_REAL Vr[3], const PQP_REAL V[3])
{
  Vr[0] = V[0];  Vr[1] = V[1];  Vr[2] = V[2];
}

inline
void
McolcV(PQP_REAL Vr[3], const PQP_REAL M[3][3], int c)
{
  Vr[0] = M[0][c];
  Vr[1] = M[1][c];
  Vr[2] = M[2][c];
}

inline
void
McolcMcol(PQP_REAL Mr[3][3], int cr, const PQP_REAL M[3][3], int c)
{
  Mr[0][cr] = M[0][c];
  Mr[1][cr] = M[1][c];
  Mr[2][cr] = M[2][c];
}

inline
void
MxMpV(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3], const PQP_REAL T[3])
{
  Mr[0][0] = (M1[0][0] * M2[0][0] +
	      M1[0][1] * M2[1][0] +
	      M1[0][2] * M2[2][0] +
	      T[0]);
  Mr[1][0] = (M1[1][0] * M2[0][0] +
	      M1[1][1] * M2[1][0] +
	      M1[1][2] * M2[2][0] +
	      T[1]);
  Mr[2][0] = (M1[2][0] * M2[0][0] +
	      M1[2][1] * M2[1][0] +
	      M1[2][2] * M2[2][0] +
	      T[2]);
  Mr[0][1] = (M1[0][0] * M2[0][1] +
	      M1[0][1] * M2[1][1] +
	      M1[0][2] * M2[2][1] +
	      T[0]);
  Mr[1][1] = (M1[1][0] * M2[0][1] +
	      M1[1][1] * M2[1][1] +
 	      M1[1][2] * M2[2][1] +
	      T[1]);
  Mr[2][1] = (M1[2][0] * M2[0][1] +
	      M1[2][1] * M2[1][1] +
	      M1[2][2] * M2[2][1] +
	      T[2]);
  Mr[0][2] = (M1[0][0] * M2[0][2] +
	      M1[0][1] * M2[1][2] +
	      M1[0][2] * M2[2][2] +
	      T[0]);
  Mr[1][2] = (M1[1][0] * M2[0][2] +
	      M1[1][1] * M2[1][2] +
	      M1[1][2] * M2[2][2] +
	      T[1]);
  Mr[2][2] = (M1[2][0] * M2[0][2] +
	      M1[2][1] * M2[1][2] +
	      M1[2][2] * M2[2][2] +
	      T[2]);
}

inline
void
MxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
{
  Mr[0][0] = (M1[0][0] * M2[0][0] +
	      M1[0][1] * M2[1][0] +
	      M1[0][2] * M2[2][0]);
  Mr[1][0] = (M1[1][0] * M2[0][0] +
	      M1[1][1] * M2[1][0] +
	      M1[1][2] * M2[2][0]);
  Mr[2][0] = (M1[2][0] * M2[0][0] +
	      M1[2][1] * M2[1][0] +
	      M1[2][2] * M2[2][0]);
  Mr[0][1] = (M1[0][0] * M2[0][1] +
	      M1[0][1] * M2[1][1] +
	      M1[0][2] * M2[2][1]);
  Mr[1][1] = (M1[1][0] * M2[0][1] +
	      M1[1][1] * M2[1][1] +
 	      M1[1][2] * M2[2][1]);
  Mr[2][1] = (M1[2][0] * M2[0][1] +
	      M1[2][1] * M2[1][1] +
	      M1[2][2] * M2[2][1]);
  Mr[0][2] = (M1[0][0] * M2[0][2] +
	      M1[0][1] * M2[1][2] +
	      M1[0][2] * M2[2][2]);
  Mr[1][2] = (M1[1][0] * M2[0][2] +
	      M1[1][1] * M2[1][2] +
	      M1[1][2] * M2[2][2]);
  Mr[2][2] = (M1[2][0] * M2[0][2] +
	      M1[2][1] * M2[1][2] +
	      M1[2][2] * M2[2][2]);
}


inline
void
MxMT(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
{
  Mr[0][0] = (M1[0][0] * M2[0][0] +
	      M1[0][1] * M2[0][1] +
	      M1[0][2] * M2[0][2]);
  Mr[1][0] = (M1[1][0] * M2[0][0] +
	      M1[1][1] * M2[0][1] +
	      M1[1][2] * M2[0][2]);
  Mr[2][0] = (M1[2][0] * M2[0][0] +
	      M1[2][1] * M2[0][1] +
	      M1[2][2] * M2[0][2]);
  Mr[0][1] = (M1[0][0] * M2[1][0] +
	      M1[0][1] * M2[1][1] +
	      M1[0][2] * M2[1][2]);
  Mr[1][1] = (M1[1][0] * M2[1][0] +
	      M1[1][1] * M2[1][1] +
 	      M1[1][2] * M2[1][2]);
  Mr[2][1] = (M1[2][0] * M2[1][0] +
	      M1[2][1] * M2[1][1] +
	      M1[2][2] * M2[1][2]);
  Mr[0][2] = (M1[0][0] * M2[2][0] +
	      M1[0][1] * M2[2][1] +
	      M1[0][2] * M2[2][2]);
  Mr[1][2] = (M1[1][0] * M2[2][0] +
	      M1[1][1] * M2[2][1] +
	      M1[1][2] * M2[2][2]);
  Mr[2][2] = (M1[2][0] * M2[2][0] +
	      M1[2][1] * M2[2][1] +
	      M1[2][2] * M2[2][2]);
}

inline
void
MTxM(PQP_REAL Mr[3][3], const PQP_REAL M1[3][3], const PQP_REAL M2[3][3])
{
  Mr[0][0] = (M1[0][0] * M2[0][0] +
	      M1[1][0] * M2[1][0] +
	      M1[2][0] * M2[2][0]);
  Mr[1][0] = (M1[0][1] * M2[0][0] +
	      M1[1][1] * M2[1][0] +
	      M1[2][1] * M2[2][0]);
  Mr[2][0] = (M1[0][2] * M2[0][0] +
	      M1[1][2] * M2[1][0] +
	      M1[2][2] * M2[2][0]);
  Mr[0][1] = (M1[0][0] * M2[0][1] +
	      M1[1][0] * M2[1][1] +
	      M1[2][0] * M2[2][1]);
  Mr[1][1] = (M1[0][1] * M2[0][1] +
	      M1[1][1] * M2[1][1] +
 	      M1[2][1] * M2[2][1]);
  Mr[2][1] = (M1[0][2] * M2[0][1] +
	      M1[1][2] * M2[1][1] +
	      M1[2][2] * M2[2][1]);
  Mr[0][2] = (M1[0][0] * M2[0][2] +
	      M1[1][0] * M2[1][2] +
	      M1[2][0] * M2[2][2]);
  Mr[1][2] = (M1[0][1] * M2[0][2] +
	      M1[1][1] * M2[1][2] +
	      M1[2][1] * M2[2][2]);
  Mr[2][2] = (M1[0][2] * M2[0][2] +
	      M1[1][2] * M2[1][2] +
	      M1[2][2] * M2[2][2]);
}

inline
void
MxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
{
  Vr[0] = (M1[0][0] * V1[0] +
	   M1[0][1] * V1[1] + 
	   M1[0][2] * V1[2]);
  Vr[1] = (M1[1][0] * V1[0] +
	   M1[1][1] * V1[1] + 
	   M1[1][2] * V1[2]);
  Vr[2] = (M1[2][0] * V1[0] +
	   M1[2][1] * V1[1] + 
	   M1[2][2] * V1[2]);
}


inline
void
MxVpV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
{
  Vr[0] = (M1[0][0] * V1[0] +
	   M1[0][1] * V1[1] + 
	   M1[0][2] * V1[2] + 
	   V2[0]);
  Vr[1] = (M1[1][0] * V1[0] +
	   M1[1][1] * V1[1] + 
	   M1[1][2] * V1[2] + 
	   V2[1]);
  Vr[2] = (M1[2][0] * V1[0] +
	   M1[2][1] * V1[1] + 
	   M1[2][2] * V1[2] + 
	   V2[2]);
}


inline
void
sMxVpV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3], const PQP_REAL V2[3])
{
  Vr[0] = s1 * (M1[0][0] * V1[0] +
		M1[0][1] * V1[1] + 
		M1[0][2] * V1[2]) +
		V2[0];
  Vr[1] = s1 * (M1[1][0] * V1[0] +
		M1[1][1] * V1[1] + 
		M1[1][2] * V1[2]) + 
		V2[1];
  Vr[2] = s1 * (M1[2][0] * V1[0] +
		M1[2][1] * V1[1] + 
		M1[2][2] * V1[2]) + 
		V2[2];
}

inline
void
MTxV(PQP_REAL Vr[3], const PQP_REAL M1[3][3], const PQP_REAL V1[3])
{
  Vr[0] = (M1[0][0] * V1[0] +
	   M1[1][0] * V1[1] + 
	   M1[2][0] * V1[2]); 
  Vr[1] = (M1[0][1] * V1[0] +
	   M1[1][1] * V1[1] + 
	   M1[2][1] * V1[2]);
  Vr[2] = (M1[0][2] * V1[0] +
	   M1[1][2] * V1[1] + 
	   M1[2][2] * V1[2]); 
}

inline
void
sMTxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
{
  Vr[0] = s1*(M1[0][0] * V1[0] +
	      M1[1][0] * V1[1] + 
	      M1[2][0] * V1[2]); 
  Vr[1] = s1*(M1[0][1] * V1[0] +
	      M1[1][1] * V1[1] + 
	      M1[2][1] * V1[2]);
  Vr[2] = s1*(M1[0][2] * V1[0] +
	      M1[1][2] * V1[1] + 
	      M1[2][2] * V1[2]); 
}

inline
void
sMxV(PQP_REAL Vr[3], PQP_REAL s1, const PQP_REAL M1[3][3], const PQP_REAL V1[3])
{
  Vr[0] = s1*(M1[0][0] * V1[0] +
	      M1[0][1] * V1[1] + 
	      M1[0][2] * V1[2]); 
  Vr[1] = s1*(M1[1][0] * V1[0] +
	      M1[1][1] * V1[1] + 
	      M1[1][2] * V1[2]);
  Vr[2] = s1*(M1[2][0] * V1[0] +
	      M1[2][1] * V1[1] + 
	      M1[2][2] * V1[2]); 
}


inline
void
VmV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])
{
  Vr[0] = V1[0] - V2[0];
  Vr[1] = V1[1] - V2[1];
  Vr[2] = V1[2] - V2[2];
}

inline
void
VpV(PQP_REAL Vr[3], const PQP_REAL V1[3], const PQP_REAL V2[3])