CodeArchive 3DGeometry

Extracted from the robot problem.

#include <iostream>
#include <cmath>
#include <string>
#include <algorithm>

using namespace std;

typedef long double ld_t;
const ld_t PI = 4 * atan(1.0);
const ld_t D2R = PI / 180;
const ld_t EPS = 1e-7;

struct vector_t {
  ld_t x, y, z, w;
  vector_t(ld_t xx = 0, ld_t yy = 0, ld_t zz = 0, ld_t ww = 1): x(xx), y(yy), z(zz), w(ww) {}
  vector_t operator+(const vector_t& a) const { // Only make sense when at least one of them is a directional vector
    return vector_t(x + a.x, y + a.y, z + a.z, w + a.w);
  }

  vector_t operator-(const vector_t& a) const {
    return vector_t(x - a.x, y - a.y, z - a.z, w - a.w);
  }

  ld_t operator*(const vector_t& a) const { // Ignore w
    return x * a.x + y * a.y + z * a.z;
  }

  vector_t operator*(ld_t scale) const {
    return vector_t(x*scale, y*scale, z*scale, w); // w is unchanged, or everything is the same.
  }

  vector_t operator/(const vector_t& a) const { // w is ignored
    return vector_t(y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x, 0);
  }

  vector_t std() const { // Make w 0 or 1
    if (w > EPS && fabs(w - 1.0) > EPS ) return vector_t(x / w, y / w, z / w, 1);
    return *this; // w is zero or w is 1
  }

  vector_t normalize() const { // Make the unit 1, only work for directional vector
    ld_t mag = sqrt(x*x+y*y+z*z);
    return vector_t(x / mag, y / mag, z / mag, 0);
  }

  ld_t mag() const {
    ld_t result = sqrt(x*x+y*y+z*z);
    if (w > EPS) result = result / w / w;
    return result;
  }
};

struct matrix_t {
  ld_t data[4][4];
  matrix_t() {
    memset(data, 0, sizeof(data));
    data[0][0] = data[1][1] = data[2][2] = data[3][3] = 1.0; // Identity
  }
  inline ld_t* operator[] (int k) {
    return data[k];
  }
  vector_t operator* (const vector_t& a) const {
    return vector_t(
        data[0][0] * a.x + data[0][1] * a.y + data[0][2] * a.z + data[0][3] * a.w,
        data[1][0] * a.x + data[1][1] * a.y + data[1][2] * a.z + data[1][3] * a.w,
        data[2][0] * a.x + data[2][1] * a.y + data[2][2] * a.z + data[2][3] * a.w,
        data[3][0] * a.x + data[3][1] * a.y + data[3][2] * a.z + data[3][3] * a.w);
  }
  matrix_t operator* (const matrix_t& a) const {
    matrix_t result;
    memset(result.data, 0, sizeof(result.data)); // Set zero
    for (int i = 0; i < 4; ++i)
      for (int j = 0; j < 4; ++j)
        for (int k = 0; k < 4; ++k)
          result.data[i][j] += data[i][k] * a.data[k][j];
    return result;
  }
};

ostream& operator<<(ostream& os, const vector_t& v) {
  vector_t x(v.std());
  os << '(';
  if (fabs(v.x) > 0.0001) os << v.x; else os << 0.000;
  os << ',';
  if (fabs(v.y) > 0.0001) os << v.y; else os << 0.000;
  os << ',';
  if (fabs(v.z) > 0.0001) os << v.z; else os << 0.000;
  os << ')';
  return os;
}

// Find the distance between two points
inline ld_t dist_pp(const vector_t& a, const vector_t& b) {
  return (a.std()-b.std()).mag();
}

// Find the distance between a point with a line, indicated by a and b
inline ld_t dist_pl(const vector_t& p, const vector_t& a, const vector_t& b) {
  return ((b.std()-a.std()).normalize() / (p.std() - a.std())).mag();
}

// Find the distance between a point and a line segment, end points at a,b
inline ld_t dist_pls(const vector_t& p, const vector_t& a, const vector_t& b) {
  vector_t dir(b.std() - a.std());
  vector_t std_p(p.std());
  if ( (std_p - a.std()) * dir <  EPS ) return dist_pp(p,a);
  if ( (std_p - b.std()) * dir > -EPS ) return dist_pp(p,b);
  return dist_pl(p, a, b);
}

// Find the min distance between two lines
inline ld_t dist_ll(const vector_t& a, const vector_t& b, const vector_t& c, const vector_t& d) {
  vector_t a_dir(( b.std() - a.std() ).normalize()), c_dir(( d.std() - c.std() ).normalize());
  vector_t dadc = a_dir / c_dir;
  if (dadc.mag() < EPS) return dist_pl(a, c, d); // Degenerate
  dadc = dadc.normalize();
  vector_t acda = (c.std() - a.std()) / a_dir;
  ld_t sqr_result = (acda * dadc);
  sqr_result = acda*acda - sqr_result*sqr_result;
  if (fabs(sqr_result) < EPS) return 0;
  return sqrt(sqr_result);
}

// Find the min distance between two line segments
inline ld_t dist_lsls(const vector_t& a, const vector_t& b, const vector_t& c, const vector_t& d) {
  vector_t ac(c.std() - a.std()), ad (d.std() - a.std()), bc(c.std() - b.std()), bd(d.std() - b.std());
  vector_t ab(b.std() - a.std()), cd (d.std() - c.std());
  if (  (ad/ab) * (ac/ab) < -EPS
      &&(ac/cd) * (bc/cd) < -EPS) // Special case!
    return dist_ll(a,b,c,d);
  return min(
      min( dist_pls(a,c,d), dist_pls(b,c,d) ),
      min( dist_pls(c,a,b), dist_pls(d,a,b) ) );
}

matrix_t translate(const ld_t &dx, const ld_t &dy, const ld_t &dz) {
  matrix_t result;
  result[0][3] = dx;
  result[1][3] = dy;
  result[2][3] = dz;
  return result;
}

matrix_t rotate_x(const ld_t &angle) {
  matrix_t result;
  ld_t ca = cos(angle), sa = sin(angle);
  result[1][1] = ca; result[2][1] = sa;
  result[1][2] = -sa; result[2][2] = ca;
  return result;
}

matrix_t rotate_y(const ld_t &angle) {
  matrix_t result;
  ld_t ca = cos(angle), sa = sin(angle);
  result[0][0] = ca; result[2][0] = -sa;
  result[0][2] = sa; result[2][2] = ca;
  return result;
}