Initial commit

1 files changed, 395 insertions, 0 deletions

diff --git a/tksao/vector/vector3d.h b/tksao/vector/vector3d.h
new file mode 100644
index 0000000..03aeb58
--- /dev/null
+++ b/tksao/vector/vector3d.h
@@ -0,0 +1,395 @@
+// Copyright (C) 1999-2016
+// Smithsonian Astrophysical Observatory, Cambridge, MA, USA
+// For conditions of distribution and use, see copyright notice in "copyright"
+
+#ifndef __vector3d_h__
+#define __vector3d_h__
+
+#include <math.h>
+#include <float.h>
+
+#include <iostream>
+using namespace std;
+
+class Vector;
+class Matrix;
+class Matrix3d;
+
+class Vector3d {
+ public:
+  double v[4];
+
+ public:
+  Vector3d() 
+    {v[0]=0;v[1]=0;v[2]=0;v[3]=1;}
+  Vector3d(double* f) 
+    {v[0]=f[0]; v[1]=f[1]; v[2]=f[2]; v[3]=1;}
+  Vector3d(double x, double y) 
+    {v[0]=x;v[1]=y;v[2]=0;v[3]=1;}
+  Vector3d(double x, double y, double z) 
+    {v[0]=x;v[1]=y;v[2]=z;v[3]=1;}
+
+  Vector3d(const Vector&);
+  Vector3d(const Vector&, double);
+  Vector3d& operator=(const Vector&);
+  
+  Vector3d(const Vector3d& a) 
+    {v[0]=a.v[0];v[1]=a.v[1];v[2]=a.v[2];v[3]=a.v[3];}
+  Vector3d& operator=(const Vector3d& a) 
+    {v[0]=a.v[0];v[1]=a.v[1];v[2]=a.v[2];v[3]=a.v[3]; return *this;}
+
+  double& operator[](int i) {return v[i];} // return element
+  double* vv() {return v;} // return vector
+
+  Vector3d& operator+=(const Vector3d& a) // addition
+    {v[0]+=a.v[0]; v[1]+=a.v[1]; v[2]+=a.v[2]; return *this;}
+  Vector3d& operator-=(const Vector3d& a) // subtraction
+    {v[0]-=a.v[0]; v[1]-=a.v[1]; v[2]-=a.v[2]; return *this;}
+  Vector3d& operator*=(double f) // scalar multiply
+    {v[0]*=f; v[1]*=f; v[2]*=f; return *this;}
+  Vector3d& operator/=(double f) // scalar division
+    {v[0]/=f; v[1]/=f; v[2]/=f; return *this;}
+  Vector3d& operator*=(const Matrix3d&); // vector multiply
+
+  Vector3d abs()
+  {return Vector3d(fabs(v[0]),fabs(v[1]),fabs(v[2]));}
+  double angleX() 
+  {return atan2(v[2],v[1]);}
+  double angleY() 
+  {return atan2(v[0],v[2]);}
+  double angleZ() 
+  {return atan2(v[1],v[0]);}
+  Vector3d ceil()
+  {return Vector3d(::ceil(v[0]),::ceil(v[1]),::ceil(v[2]));}
+  Vector3d floor()
+  {return Vector3d(::floor(v[0]),::floor(v[1]),::floor(v[2]));}
+  Vector3d invert()
+  {return Vector3d(1/v[0],1/v[1],1/v[2]);}
+  double length()
+  {return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);}
+  Vector3d round()
+  {return Vector3d((int)(v[0]+.5),(int)(v[1]+.5),(int)(v[2]+.5));}
+  Vector3d normalize()
+  {double d = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
+    return d ? Vector3d(v[0]/d,v[1]/d,v[2]/d) : Vector3d();}
+  Vector3d project()
+  {return (v[3]!=1) ? Vector3d(v[0]/v[3],v[1]/v[3],v[2]/v[3]) : *this;}
+  Vector TkCanvasPs(void* canvas);
+};
+ostream& operator<<(ostream&, const Vector3d&);
+istream& operator>>(istream&, Vector3d&);
+
+inline Vector3d operator-(const Vector3d& a) 
+{return Vector3d(-a.v[0],-a.v[1],-a.v[2]);}
+inline Vector3d operator+(const Vector3d& a, const Vector3d& b)
+{return Vector3d(a) +=b;}
+inline Vector3d operator-(const Vector3d& a, const Vector3d& b)
+{return Vector3d(a) -=b;}
+inline Vector3d operator*(const Vector3d& a, double b) 
+{return Vector3d(a) *=b;}
+inline Vector3d operator/(const Vector3d& a, double b)
+{return Vector3d(a) /=b;}
+inline Vector3d operator*(const Vector3d& v, const Matrix3d& m)
+{return Vector3d(v) *=m;}
+inline double operator*(const Vector3d& a, const Vector3d& b) // dot product
+{double r=0; r+=a.v[0]*b.v[0]; r+=a.v[1]*b.v[1]; r+=a.v[2]*b.v[2]; return r;}
+inline Vector3d cross(Vector3d& a, Vector3d& b) // cross product
+{return Vector3d(a[1]*b[2]-b[1]*a[2],a[2]*b[0]-b[2]*a[0],a[0]*b[1]-b[0]*a[1]);}
+
+class Vertex3d {
+public:
+  Vector3d vector;
+
+private:
+  Vertex3d* next_;
+  Vertex3d* previous_;
+
+public:
+  Vertex3d() 
+    {next_=NULL; previous_=NULL;}
+  Vertex3d(double x, double y, double z) 
+    {vector=Vector3d(x,y,z); next_=NULL; previous_=NULL;}
+  Vertex3d(const Vector3d& a) 
+    {vector=a; next_=NULL; previous_=NULL;}
+
+  Vertex3d(const Vertex3d& a) 
+    {vector=a.vector; next_=a.next_; previous_=a.previous_;}
+  Vertex3d& operator=(const Vertex3d& a)
+    {vector=a.vector; next_=a.next_; previous_=a.previous_; return *this;}
+
+  Vertex3d* next()
+  {return next_;}
+  Vertex3d* previous()
+  {return previous_;}
+  void setNext(Vertex3d* v)
+  {next_ = v;}
+  void setPrevious(Vertex3d* v)
+  {previous_ = v;}
+};
+ostream& operator<<(ostream&, const Vertex3d&);
+
+class Matrix3d {
+ public:
+  double m[4][4];
+
+ public:
+  Matrix3d()
+    { m[0][0]=1; m[0][1]=0; m[0][2]=0; m[0][3]=0;
+      m[1][0]=0; m[1][1]=1; m[1][2]=0; m[1][3]=0;
+      m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
+      m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1; }
+
+  Matrix3d(double a, double b, double c,
+	   double d, double e, double f,
+	   double g, double h, double i,
+	   double j, double k, double l)
+    { m[0][0]=a; m[0][1]=b; m[0][2]=c; m[0][3]=0;
+      m[1][0]=d; m[1][1]=e; m[1][2]=f; m[1][3]=0;
+      m[2][0]=g; m[2][1]=h; m[2][2]=i; m[2][3]=0;
+      m[3][0]=j; m[3][1]=k; m[3][2]=l; m[3][3]=1; }
+
+  Matrix3d(Vector3d& x, Vector3d& y, Vector3d& z)
+    { m[0][0]=x[0]; m[0][1]=y[0]; m[0][2]=z[0]; m[0][3]=0;
+      m[1][0]=x[1]; m[1][1]=y[1]; m[1][2]=z[1]; m[1][3]=0;
+      m[2][0]=x[2]; m[2][1]=y[2]; m[2][2]=z[2]; m[2][3]=0;
+      m[3][0]=0;    m[3][1]=0;    m[3][2]=0;    m[3][3]=1; }
+
+  Matrix3d(const Matrix3d& a)
+    { m[0][0]=a.m[0][0];m[0][1]=a.m[0][1];m[0][2]=a.m[0][2];m[0][3]=a.m[0][3];
+      m[1][0]=a.m[1][0];m[1][1]=a.m[1][1];m[1][2]=a.m[1][2];m[1][3]=a.m[1][3];
+      m[2][0]=a.m[2][0];m[2][1]=a.m[2][1];m[2][2]=a.m[2][2];m[2][3]=a.m[2][3];
+      m[3][0]=a.m[3][0];m[3][1]=a.m[3][1];m[3][2]=a.m[3][2];m[3][3]=a.m[3][3]; }
+
+  Matrix3d& operator=(const Matrix3d& a)
+    { m[0][0]=a.m[0][0];m[0][1]=a.m[0][1];m[0][2]=a.m[0][2];m[0][3]=a.m[0][3];
+      m[1][0]=a.m[1][0];m[1][1]=a.m[1][1];m[1][2]=a.m[1][2];m[1][3]=a.m[1][3];
+      m[2][0]=a.m[2][0];m[2][1]=a.m[2][1];m[2][2]=a.m[2][2];m[2][3]=a.m[2][3];
+      m[3][0]=a.m[3][0];m[3][1]=a.m[3][1];m[3][2]=a.m[3][2];m[3][3]=a.m[3][3];
+      return *this;}
+  
+  Matrix3d(const Matrix& a);
+  double matrix(int i, int j)
+  {return m[i][j];} // return element
+  Vector3d operator[](int i)
+  {return Vector3d(m[i]);} // return row
+  double* mm()
+  {return (double*)m;} // return matrix
+
+  Matrix3d& identity()
+    { m[0][0]=1; m[0][1]=0; m[0][2]=0; m[0][3]=0;
+      m[1][0]=0; m[1][1]=1; m[1][2]=0; m[1][3]=0;
+      m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
+      m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1; return *this;}
+  Matrix3d& operator*=(const Matrix3d&); // matrix multiply
+
+  Matrix3d invert();
+  Matrix3d cofactor();
+  Matrix3d adjoint();
+  double det();
+  double det2d(double& a, double& b, double& c, 
+	       double& d, double& e, double& f,
+	       double& g, double& h, double& i)
+  {return a*(e*i-f*h) - b*(d*i-f*g) + c*(d*h-e*g);}
+
+  void dump();
+};
+ostream& operator<<(ostream&, const Matrix3d&);
+istream& operator>>(istream&, Matrix3d&);
+
+inline Matrix3d operator*(const Matrix3d& a, const Matrix3d& b)
+{return Matrix3d(a) *= b;}
+inline Vector3d& Vector3d::operator*=(const Matrix3d& m)
+{
+  double vv[4];
+  double* mm = (double*)(m.m);
+  vv[0] = v[0]*mm[0] + v[1]*mm[4] + v[2]*mm[8]  + v[3]*mm[12]; 
+  vv[1] = v[0]*mm[1] + v[1]*mm[5] + v[2]*mm[9]  + v[3]*mm[13]; 
+  vv[2] = v[0]*mm[2] + v[1]*mm[6] + v[2]*mm[10] + v[3]*mm[14];
+  vv[3] = v[0]*mm[3] + v[1]*mm[7] + v[2]*mm[11] + v[3]*mm[15];
+  v[0] = vv[0];
+  v[1] = vv[1];
+  v[2] = vv[2];
+  v[3] = vv[3];
+  return *this;
+}
+
+class Translate3d : public Matrix3d {
+public:
+  Translate3d() {};
+  Translate3d(double x, double y, double z) 
+    {m[3][0]=x; m[3][1]=y; m[3][2]=z;}
+  Translate3d(const Vector3d& v) 
+    {m[3][0]=v.v[0]; m[3][1]=v.v[1]; m[3][2]=v.v[2];}
+  Translate3d(const Vector& v);
+  Translate3d(const Vector& v, double z);
+  Translate3d(const Matrix3d& a)
+    {m[3][0] = a.m[3][0]; m[3][1] = a.m[3][1]; m[3][2] = a.m[3][2];}
+};
+ostream& operator<<(ostream&, const Translate3d&);
+istream& operator>>(istream&, Translate3d&);
+
+class Scale3d : public Matrix3d {
+public:
+  Scale3d() {};
+  Scale3d(double a)
+    {m[0][0]=a; m[1][1]=a; m[2][2]=a;}
+  Scale3d(double a, double b)
+    {m[0][0]=a; m[1][1]=a; m[2][2]=b;}
+  Scale3d(double a, double b, double c)
+    {m[0][0]=a; m[1][1]=b; m[2][2]=c;}
+  Scale3d(const Vector& v);
+  Scale3d(const Vector& v, double c);
+  Scale3d(const Vector3d& v)
+    {m[0][0]=v.v[0]; m[1][1]=v.v[1]; m[2][2]=v.v[2];}
+  Scale3d(const Matrix3d& a) 
+    {m[0][0] = a.m[0][0]; m[1][1] = a.m[1][1]; m[2][2] = a.m[2][2];}
+};
+ostream& operator<<(ostream&, const Scale3d&);
+istream& operator>>(istream&, Scale3d&);
+
+class FlipX3d : public Matrix3d {
+public:
+  FlipX3d() 
+    {m[0][0] = -1;}
+};
+
+class FlipY3d : public Matrix3d {
+public:
+  FlipY3d() 
+    {m[1][1] = -1;}
+};
+
+class FlipZ3d : public Matrix3d {
+public:
+  FlipZ3d() 
+    {m[2][2] = -1;}
+};
+
+class FlipXY3d : public Matrix3d {
+public:
+  FlipXY3d() 
+    {m[0][0] = -1; m[1][1] = -1;}
+};
+
+class FlipXYZ3d : public Matrix3d {
+public:
+  FlipXYZ3d() 
+    {m[0][0] = -1; m[1][1] = -1; m[2][2] = -1;}
+};
+
+class RotateX3d : public Matrix3d {
+public:
+  RotateX3d()
+    {};
+  RotateX3d(double);
+  RotateX3d(double a, double b, double c, double d)
+    {m[1][1] = a; m[1][2] = b; m[2][1] = c; m[2][2] = d;}
+};
+ostream& operator<<(ostream&, const RotateX3d&);
+istream& operator>>(istream&, RotateX3d&);
+
+class RotateY3d : public Matrix3d {
+public:
+  RotateY3d()
+    {};
+  RotateY3d(double);
+  RotateY3d(double a, double b, double c, double d)
+    {m[0][0] = a; m[0][2] = b; m[2][0] = c; m[2][2] = d;}
+};
+ostream& operator<<(ostream&, const RotateY3d&);
+istream& operator>>(istream&, RotateY3d&);
+
+class RotateZ3d : public Matrix3d {
+public:
+  RotateZ3d()
+    {};
+  RotateZ3d(double);
+  RotateZ3d(double a, double b, double c, double d)
+    {m[0][0] = a; m[0][1] = b; m[1][0] = c; m[1][1] = d;}
+};
+ostream& operator<<(ostream&, const RotateZ3d&);
+istream& operator>>(istream&, RotateZ3d&);
+
+class Shear3d : public Matrix3d {
+ public:
+  Shear3d(double x, double y, double dist)
+    {m[2][0] = -x/dist; m[2][1] = -y/dist;}
+};
+
+class Perspective3d : public Matrix3d {
+ public:
+  Perspective3d(double front, double back, double dist)
+  { m[2][2] = back/(back-front); m[2][3] = 1; 
+    m[3][2] = -front*back/(dist*(back-front)); m[3][3] = 0;}
+};
+
+class BBox3d {
+ public:
+  Vector3d ll;
+  Vector3d ur;
+
+ public:
+  BBox3d()
+    {}
+  BBox3d(double w, double h, double d) 
+    {ll.v[0]=0; ll.v[1]=0; ll.v[2]=0; ur.v[0]=w; ur.v[1]=h; ur.v[2]=d;}
+  BBox3d(const Vector3d& v) 
+    {ll=v; ur=v;}
+  BBox3d(double, double, double, double, double, double);
+  BBox3d(const Vector3d&, const Vector3d&);
+
+  BBox3d(const BBox3d& a)
+    {ll=a.ll; ur=a.ur;}
+  BBox3d& operator=(const BBox3d& a)
+    {ll=a.ll; ur=a.ur; return *this;}
+
+  BBox3d& operator+=(const Vector3d& v) // addition
+    {ll+=v; ur+=v; return *this;}
+  BBox3d& operator-=(const Vector3d& a) // subtraction
+    {ll-=a; ur-=a; return *this;}
+  BBox3d& operator*=(const Matrix3d& m) // multiplication
+    {ll*=m; ur*=m; return *this;} 
+
+  double volume();
+  Vector3d center() 
+  {return (ur-ll)/2 + ll;}
+  Vector3d size() 
+  {return ur - ll;}
+  int isEmpty() const 
+  {Vector3d v = ur-ll; return (v[0]==0 && v[1]==0 && v[2]==0);}
+  int isIn(const Vector3d&) const;
+
+  BBox3d& expand(double a) 
+    {ll-=Vector3d(a,a,a); ur+=Vector3d(a,a,a); return *this;}
+  BBox3d& expand(const Vector3d& v) 
+    {ll-=v; ur+=v; return *this;}
+  BBox3d& shrink(double a) 
+    {ll+=Vector3d(a,a,a); ur-=Vector3d(a,a,a); return *this;}
+  BBox3d& shrink(const Vector3d& v) 
+    {ll+=v; ur-=v; return *this;}
+  // expand bbox3d to include vector3d
+  BBox3d& bound(const Vector3d&);
+};
+ostream& operator<<(ostream&, const BBox3d&);
+
+inline BBox3d operator+(const BBox3d& b, const Vector3d& v)
+{return BBox3d(b) += v;}
+inline BBox3d operator-(const BBox3d& b, const Vector3d& v)
+{return BBox3d(b) -= v;}
+inline BBox3d operator*(const BBox3d& b, const Matrix3d& m)
+{return BBox3d(b) *= m;}
+
+// WorldToView
+
+Matrix3d WorldToView3d(const Vector3d& cop, 
+		       const Vector3d& vpn, 
+		       const Vector3d& vup);
+Matrix3d WorldToView3d(const Vector3d& cop, 
+		       double head, double pitch, double bank);
+Matrix3d WorldToView3d(const Vector3d& cop, const Vector3d& vpn, double bank);
+
+#endif
+
+
+
+