vect.h

Go to the documentation of this file.

// SPDX-FileCopyrightText: Copyright (c) NumeriCor GmbH
// SPDX-License-Identifier: Apache-2.0
 
#ifndef VECT_H
#define VECT_H
 
#include <algorithm>
#include <cstdio>
#include <cmath>
 
namespace opencarp {
 
#define POINT_REAL double
 
template<typename V>
struct vec3 {
  V x, y, z;
 
  vec3() : x(0), y(0), z(0)
  {}
 
  template<typename S>
  void assign(S ix, S iy, S iz)
  {x = ix, y = iy, z = iz;}
 
  template<typename S>
  vec3(S ix, S iy, S iz) : x(ix), y(iy), z(iz)
  {}
  template<class VEC>
  vec3(VEC & v) {
    x = v.x; y = v.y; z = v.z;
  }
  template<typename S>
  void operator= (const S* v)
  {
    x = v[0], y = v[1], z = v[2];
  }
  template<typename S>
  void operator= (const vec3<S> & v)
  {
    x = v.x, y = v.y, z = v.z;
  }
 
  void operator+= (const vec3<V> & v)
  {
    x += v.x;
    y += v.y;
    z += v.z;
  }
  void operator-= (const vec3<V> & v)
  {
    x -= v.x;
    y -= v.y;
    z -= v.z;
  }
  void operator*= (const V s)
  {
    x *= s;
    y *= s;
    z *= s;
  }
 
  void operator/= (const V s)
  {
    x /= s;
    y /= s;
    z /= s;
  }
};
 
typedef vec3<POINT_REAL> Point;
 
template<typename V>
inline V det3(const vec3<V> & a, const vec3<V> & b, const vec3<V> & c) /* return determinate of 3x3 matrix */
{
    return (a.x*b.y*c.z + a.y*b.z*c.x + a.z*b.x*c.y - a.x*b.z*c.y - a.y*b.x*c.z - a.z*b.y*c.x);
}
 
template<typename V>
inline V dist_2(const vec3<V> & p1, const vec3<V> & p2)     /* square of distance between points*/
{
  V d =
    (p1.x-p2.x)*(p1.x-p2.x) +
    (p1.y-p2.y)*(p1.y-p2.y) +
    (p1.z-p2.z)*(p1.z-p2.z);
 
  if (d<0 || d!= d)   return 0. ;
  else                  return d  ;
}
 
template<typename V>
inline V dist(const vec3<V> & p1, const vec3<V> & p2)       /* distance between points*/
{
  return  sqrt(dist_2(p1, p2));
}
 
template<typename V>
inline V angle(const vec3<V> & v1, const vec3<V> & v2) { /* return angle between vectors in rad*/
  return fabs(dot(v1, v2)) / (mag(v1)*mag(v2));
}
 
template<typename V>
inline V dot(const vec3<V> & p1, const vec3<V> & p2)        /* perform dot prduct operation */
{
  return p1.x*p2.x + p1.y*p2.y + p1.z*p2.z;
}
 
template<typename V>
inline V mag(const vec3<V> & vect)  /* determine magnitude of a vector */
{
  return sqrt(vect.x*vect.x + vect.y*vect.y + vect.z*vect.z);
}
 
 
template<typename V>
inline V mag2(const vec3<V> & vect)  /* determine magnitude of a vector */
{
  return vect.x*vect.x + vect.y*vect.y + vect.z*vect.z;
}
 
template<typename V>
inline vec3<V> cross(const vec3<V> & a, const vec3<V> & b)         /* cross product aXb */
{
  return {a.y*b.z - b.y*a.z, b.x*a.z - a.x*b.z, a.x*b.y - a.y*b.x};
}
 
template<typename V, typename S>
inline vec3<V> scal_X(const vec3<V> & a, S k)/* scalar multiplication of vector a by k */
{
  return {a.x * k, a.y * k, a.z * k};
}
 
template<typename V>
inline vec3<V> operator* (const vec3<V> & a, V k)/* scalar multiplication of vector a by k */
{
  return {a.x * k, a.y * k, a.z * k};
}
 
template<typename V>
inline vec3<V> operator/ (const vec3<V> & a, V k)/* scalar multiplication of vector a by k */
{
  return {a.x / k, a.y / k, a.z / k};
}
 
template<typename V>
inline vec3<V> scale3(const vec3<V> & a, const vec3<V> & k) /* anisotropic scalar mult. of vector a */
{
  vec3<V> r = a;
  r.x *= k.x;
  r.y *= k.y;
  r.z *= k.z;
 
  return r;
}
 
template<typename V>
inline vec3<V> operator- (const vec3<V> & a, const vec3<V> & b)
{
  vec3<V> r;
  r.x = a.x - b.x;
  r.y = a.y - b.y;
  r.z = a.z - b.z;
  return r;
}
template<typename V>
inline vec3<V> operator+ (const vec3<V> & a, const vec3<V> & b)
{
  vec3<V> r;
  r.x = a.x + b.x;
  r.y = a.y + b.y;
  r.z = a.z + b.z;
  return r;
}
 
template<typename V>
inline vec3<V> normalize(vec3<V> a)          /* return unit vector in direction of a */
{
  double magnitude = mag(a);
  a.x /= magnitude;
  a.y /= magnitude;
  a.z /= magnitude;
  return a;
}
 
 
template<typename V>
inline double *v_scale(int n, double *a, double k, double *b)
/*scale vector by a constant*/
{
  for (n--; n>=0; n--)
    b[n] = k*a[n];
 
  return b;
}
 
 
template<typename V>
inline float *v_scale_f(int n, float *a, double k, double *b)
/*scale vector by a constant*/
{
  for (n--; n>=0; n--)
    b[n] = k*a[n];
 
  return a;
}
 
 
template<typename V>
inline vec3<V> mid_point(vec3<V> a, vec3<V> b)  /* return midpoint of line ab */
{
  return {(a.x + b.x)/2.0, (a.y + b.y)/2.0, (a.z + b.z)/2.0};
}
 
 
template<typename V>
inline vec3<V> rotate_z(vec3<V> p, double theta)        /* rotate about z_axis */
{
  double cost = cos(theta);
  double sint = sin(theta);
  return {cost*p.x+sint*p.y, -sint*p.x+cost*p.y, p.z};
}
 
 
template<typename V>
inline vec3<V> rotate_y(vec3<V> p, double theta)   /* rotate about y_axis */
{
  double cost = cos(theta);
  double sint = sin(theta);
  return {cost*p.x-sint*p.z, p.y, sint*p.x+cost*p.z};
}
 
 
template<typename V>
inline vec3<V> rotate_x(vec3<V> p, double theta)   /* rotate about x_axis */
{
  double cost = cos(theta);
  double sint = sin(theta);
  return { p.x, cost*p.y+sint*p.z, -sint*p.y+cost*p.z};
}
 
 
inline void matrix_mathematica(int n, double **a, char *outfile)
/* output a matrix in matematica form */
{
  FILE *out;
  int i, j;
 
  out = fopen(outfile, "w");
  fprintf(out, "%s = {\n", outfile);
  for (i=0 ; i<n; i++) {
    fprintf(out, "{ ");
    for (j=0 ; j<n; j++) {
      fprintf(out, "%.12f 10^%.0f", a[i][j] != 0.0 ?
               a[i][j]/pow(10.0, floor(log10(fabs(a[i][j])))) : 0.0, 
               a[i][j] != 0.0 ?floor(log10(fabs(a[i][j])))  : 0.0) ;
      if (j != n-1)
        fprintf(out, ", \n");
    }
    fprintf(out, "}");
    if (i!= n-1)
      fprintf(out, ", ");
    fprintf(out, "\n");
  }
  fprintf(out, "}\n");
  fclose(out);
}
 
template<typename V>
inline vec3<V> p_project(vec3<V> A, vec3<V> B)
{
  return scal_X(A, dot(normalize(A), normalize(B)));
}
 
 
template<typename V>
inline vec3<V>  p_transverse(vec3<V> A, vec3<V> B)
{
  return p_sub(A, p_project(A, B));
}
 
template<typename V>
inline double triple_prod(vec3<V> a, vec3<V> b, vec3<V> c)
{
  return dot(a, cross(b, c));
}
 
template<typename V>
inline bool isNaN(vec3<V> a)
{
  return ((a.x != a.x) || (a.y != a.y) || (a.z != a.z));
}
 
// inf norm of a point
template<typename V>
inline double infnorm(const vec3<V> & a)
{
  double max = std::abs(a.x);
  max = std::max(std::abs(a.y),max);
  max = std::max(std::abs(a.z),max);
  return max;
}
 
}  // namespace opencarp
 
#endif