增加CNC运行测试

2013-07-11 22:08:30 +08:00
parent d5bfc97a9a
commit 2e237bf90f
43 changed files with 4393 additions and 48 deletions
@@ -0,0 +1,729 @@
+#include "stdafx.h"
+#include <cmath>
+# include "CPoint3.h"
+
+
+#define ABS(x) ((x)>=0?(x):-(x))
+#define NORM2_EPSILON 1e-64
+#define NORM1_EPSILON 1e-32
+
+#define CPOINT3_CPP
+
+CPoint3::CPoint3(void): CObject ()
+{
+  //cout << _T( "creating object " ) << this << _T( " with default constructor\n" );
+  x = y = z = 0.0;
+}
+
+CPoint3::~CPoint3(void)
+{
+  //cout << _T( "Deleting object " ) << this << _T( "\n" );
+}
+
+CPoint3::CPoint3(const double* t): CObject ()
+{
+  //cout << _T( "creating object " ) << this << _T( " with double * constructor using data " );
+  //cout << t[0] << _T( ", " ) << t[1] << _T( ", " ) << t[2] << _T( "\n" );
+  if (t)
+  {
+    x = t[0];
+    y = t[1];
+    z = t[2];
+  }
+  else
+  {
+    x = y = z = 0;
+  }
+}
+
+CPoint3::CPoint3(const double xx, const double yy, const double zz): CObject ()
+{
+  //cout << _T( "creating object " ) << this << _T( " with 3 double constructor using data " );
+  //cout << xx <<_T( ", " ) << yy << _T( ", " ) << zz << _T( "\n" );
+  x = xx;
+  y = yy;
+  z = zz;
+}
+
+CPoint3::CPoint3(const CPoint3 & t): CObject ()
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+  //cout << _T( "creating object " ) << this << _T( " with copy constructor \n" );
+  x = t.x;
+  y = t.y;
+  z = t.z;
+}
+
+CPoint3::CPoint3(const float* pts): CObject ()
+{
+  x = (double) pts[0];
+  y = (double) pts[1];
+  z = (double) pts[2];
+}
+
+CPoint3::operator const double *(void) const
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+  //cout << _T( "operator double * on " ) << this << _T( " yeilds " ) << m_data << _T( "\n" );
+  return & (x);
+}
+
+CPoint3::operator double *(void)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+  //cout << _T( "operator double * on " ) << this << _T( " yeilds " ) << m_data << _T( "\n" );
+  return & (x);
+}
+
+CPoint3 operator+(const CPoint3 &t, const CPoint3 &u)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+  ASSERT(u.IsValid());
+#endif
+
+  //cout << _T( "calling operator+ on objects " ) << &t << _T( " and " ) << &u;
+  //cout << _T( " and returning " ) << &result << _T( "\n" );
+  return CPoint3((t.x + u.x), (t.y + u.y), (t.z + u.z));
+}
+
+CPoint3 CPoint3::operator-(void)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+  return CPoint3(-x,-y,-z);
+}
+
+CPoint3 operator-(const CPoint3 &t, const CPoint3 &u)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+  ASSERT(u.IsValid());
+#endif
+
+  //cout << _T( "calling operator- on objects " ) << &t << _T( " and " ) << &u;
+  //cout << _T( " and returning " ) << &result << _T( "\n" );
+  return CPoint3((t.x - u.x), (t.y - u.y), (t.z - u.z));
+}
+
+CPoint3 & CPoint3::operator+=(const CPoint3 &t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator+= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x += t.x;
+  y += t.y;
+  z += t.z;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator+=(const double* t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+
+  //cout << _T( "calling operator+= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x += t[0];
+  y += t[1];
+  z += t[2];
+  return *this;
+}
+
+CPoint3 & CPoint3::operator+=(double t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+
+  x += t;
+  y += t;
+  z += t;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator-=(const CPoint3 &t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator-= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x -= t.x;
+  y -= t.y;
+  z -= t.z;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator*=(const CPoint3 &t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator*= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x *= t.x;
+  y *= t.y;
+  z *= t.z;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator-=(const double* t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+
+  //cout << _T( "calling operator-= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x -= t[0];
+  y -= t[1];
+  z -= t[2];
+  return *this;
+}
+
+CPoint3 & CPoint3::operator-=(double t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+#endif
+
+  x -= t;
+  y -= t;
+  z -= t;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator=(const CPoint3 & t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x = t.x;
+  y = t.y;
+  z = t.z;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator=(const float* t)
+{
+  //cout << _T( "calling operator= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  if (t)
+  {
+    x = (double) t[0];
+    y = (double) t[1];
+    z = (double) t[2];
+  }
+  else
+  {
+    x = 0.0;
+    y = 0.0;
+    z = 0.0;
+  }
+  return *this;
+}
+
+CPoint3 & CPoint3::operator=(const double* t)
+{
+  //cout << _T( "calling operator= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  if (t)
+  {
+    x = t[0];
+    y = t[1];
+    z = t[2];
+  }
+  else
+  {
+    x = 0.0;
+    y = 0.0;
+    z = 0.0;
+  }
+  return *this;
+}
+
+CPoint3 & CPoint3::operator=(const double t)
+{
+  x = t;
+  y = t;
+  z = t;
+  return *this;
+}
+
+bool CPoint3::operator==(const CPoint3 &t) const
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator== on object " ) << this << _T( " and on object " ) << &t;
+  if ((x == t.x) &&
+    (y == t.y) &&
+    (z == t.z))
+  {
+    //cout << _T( " and returning TRUE" ) << _T( "\n" );
+    return true;
+  }
+  else
+  {
+    //cout << _T( " and returning FALSE" ) << _T( "\n" );
+    return false;
+  }
+}
+
+bool CPoint3::operator!=(const CPoint3 &t) const
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator!= on object " ) << this << _T( " and on object " ) << &t;
+  if ((x == t.x) &&
+    (y == t.y) &&
+    (z == t.z))
+  {
+    //cout << _T( " and returning FALSE\n" );
+    return false;
+  }
+  else
+  {
+    //cout << _T( " and returning TRUE\n" );
+    return true;
+  }
+}
+
+
+void CPoint3::display(void)
+{
+  //cout << _T( "CPoint3 " ) << this << _T( " = " ) << x << _T( ", " ) << y << _T( ", " ) << z << _T( "\n" );
+}
+
+CPoint3 operator* (const CPoint3 &t, double mult) // scalar multiply
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator* on object " ) << t << _T( " and double " ) << mult << _T( "\n" );
+  return CPoint3(t.x *mult, t.y *mult, t.z *mult);
+}
+
+CPoint3 operator* (double mult, const CPoint3 &t) // scalar multiply
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator* on double " ) << mult << _T( " and object " ) << t << _T( "\n" );
+  return CPoint3(t.x *mult, t.y *mult, t.z *mult);
+}
+
+CPoint3 operator+(const CPoint3 &t, double mult) // add t pointwise
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  return CPoint3(t.x + mult, t.y + mult, t.z + mult);
+}
+
+CPoint3 operator+(double mult, const CPoint3 &t) // add t pointwise
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  return CPoint3(t.x + mult, t.y + mult, t.z + mult);
+}
+
+CPoint3 operator-(const CPoint3 &t, double mult) // subtract t pointwise
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  return CPoint3(t.x - mult, t.y - mult, t.z - mult);
+}
+
+CPoint3 operator-(double mult, const CPoint3 &t) // subtract t pointwise
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  return CPoint3(mult - t.x, mult - t.y, mult - t.z);
+}
+
+double operator* (const CPoint3 &t, const CPoint3 &u)   // dot product
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+  ASSERT(u.IsValid());
+#endif
+
+  //cout << _T( "calling operator* on object " ) << t << _T( " and object " ) << u << _T( "\n" );
+  return t.x *u.x +
+    t.y *u.y +
+    t.z *u.z;
+}
+
+CPoint3 & CPoint3::operator*=(double mult) // scalar multiply
+{
+  //cout << _T( "calling operator*= on object " ) << *this << _T( " and double " ) << mult << _T( "\n" );
+  x *= mult;
+  y *= mult;
+  z *= mult;
+  return *this;
+}
+
+CPoint3 operator/(const CPoint3 &t, double divisor)                // scalar divide
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator/ on object " ) << t << _T( " and double " ) << divisor << _T( "\n" );
+  CPoint3 result(0.0, 0.0, 0.0);
+  if (divisor != 0.0)
+  {
+    result.x = t.x / divisor;
+    result.y = t.y / divisor;
+    result.z = t.z / divisor;
+  }
+  return result;
+}
+
+CPoint3 & CPoint3::operator/=(const CPoint3 &t)
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(t.IsValid());
+#endif
+
+  //cout << _T( "calling operator*= on object " ) << &t;
+  //cout << _T( " and returning " ) << this << _T( "\n" );
+  x /= t.x;
+  y /= t.y;
+  z /= t.z;
+  return *this;
+}
+
+CPoint3 & CPoint3::operator/=(double divisor)              // scalar divide and assignment
+{
+  //cout << _T( "calling operator/= on object " ) << *this << _T( " and double " ) << divisor << _T( "\n" );
+  if (divisor != 0.0)
+  {
+    x /= divisor;
+    y /= divisor;
+    z /= divisor;
+  }
+  else
+  {
+    x = y = z = 0.0;
+  }
+  return *this;
+}
+
+void CPoint3::Serialize(CArchive & archive)
+{
+  CObject::Serialize(archive);
+
+  if (archive.IsStoring())
+  {
+    archive << (double) x;
+    archive << (double) y;
+    archive << (double) z;
+  }
+  else
+  {
+    archive >> (double&) x;
+    archive >> (double&) y;
+    archive >> (double&) z;
+  }
+}
+
+double CPoint3::norm(void) const              // return the norm of this object
+{
+  return sqrt(x *x + y *y + z *z);
+}
+double CPoint3::normalize()
+{
+	double Norm = norm();
+	if (Norm > 0.0)
+	{
+    double m_data[3]={0.0};
+    m_data[0]=x;
+    m_data[1]=y;
+    m_data[2]=z;
+		for (int i = 0; i < 3; i++)
+		{
+			m_data[i] /= Norm;
+		}
+	}
+	else
+	{
+		z = 1.0;
+	}
+	return (Norm);
+}
+
+/* PR272573 <Yanhua Huang> only normalize when the original vector is not approximatly normalized.
+1. return the unit original vector, [1 - norm2(vector)] < NORM2_EPSILON
+2. return the new normalized vector, ABS[norm2(vector) - 1] > NORM2_EPSILON*/
+double CPoint3::Normalize(double dbEpsilon)
+{
+	double dbNorm = 0;
+	dbNorm = norm2();
+	if (dbNorm < 1.0)
+	{
+		if( ABS(dbNorm - 1.0) < NORM2_EPSILON ) // unit vector
+		{
+			dbNorm = 1.0;
+			return dbNorm;
+		}
+	}
+	dbNorm = sqrt(dbNorm);
+	if (dbEpsilon < 0)
+		dbEpsilon = NORM1_EPSILON;
+	else if (dbEpsilon > NORM1_EPSILON)
+		dbEpsilon = NORM1_EPSILON;
+	if (dbNorm > dbEpsilon) // normalize to unit vector
+	{
+    double m_data[3]={0.0};
+    m_data[0]=x;
+    m_data[1]=y;
+    m_data[2]=z;
+		for (int i = 0; i < 3; i++)
+		{
+			m_data[i] /= dbNorm;
+		}  
+	}
+	else
+	{
+		z = 1.0;
+	}
+	return (dbNorm);
+}
+CPoint3 CPoint3::crossOnly(const CPoint3 &u) const
+{
+  return CPoint3((y*u[2] - z*u[1]), (z*u[0] - x*u[2]), (x*u[1] - y*u[0]));
+}
+
+CPoint3 CPoint3::cross(const CPoint3 &u) const // cross product
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(u.IsValid());
+#endif
+
+  CPoint3 result(0.0, 0.0, 0.0);
+
+  result[0] = y *u[2] - z *u[1];
+  result[1] = z *u[0] - x *u[2];
+  result[2] = x *u[1] - y *u[0];
+  result.normalize();
+  return (result);
+}
+
+// Anis - The above cross always return a non (0, 0, 0) result which is not always interesting
+CPoint3 CPoint3::cross2(const CPoint3 &u) const // cross product
+{
+#ifdef CPOINT3_DEBUG_CHECKS
+  ASSERT(IsValid());
+  ASSERT(u.IsValid());
+#endif
+
+  CPoint3 result;
+
+  result[0] = y *u[2] - z *u[1];
+  result[1] = z *u[0] - x *u[2];
+  result[2] = x *u[1] - y *u[0];
+
+  double Norm = result.norm();
+
+  if (Norm > 0.0)
+  {
+    for (int i = 0; i < 3; i++)
+      result[i] /= Norm;
+  }
+  return (result);
+}
+
+// Anis
+double CPoint3::norm2(void) const              // return the squared norm of this object
+{
+  return (x *x + y *y + z *z);
+}
+
+double CPoint3::maxabs(int* Index) const
+{
+  double cVal = -1.0;
+  double m_data[3]={0.0};
+  m_data[0]=x;
+  m_data[1]=y;
+  m_data[2]=z;
+  for (int i = 0; i < 3; i++)
+  {
+    if (fabs(m_data[i]) > cVal)
+    {
+      if (Index)
+      {
+        *Index = i;
+      }
+      cVal = fabs(m_data[i]);
+    }
+  }
+  return (cVal);
+}
+
+double CPoint3::minabs(int* Index) const
+{
+  double cVal = 0.0;
+  double m_data[3]={0.0};
+  m_data[0]=x;
+  m_data[1]=y;
+  m_data[2]=z;
+  for (int i = 0; i < 3; i++)
+  {
+    if (fabs(m_data[i]) < cVal || i == 0)
+    {
+      if (Index)
+      {
+        *Index = i;
+      }
+      cVal = fabs(m_data[i]);
+    }
+  }
+  return (cVal);
+}
+
+void CPoint3::setXYZ(double _x, double _y, double _z)
+{
+  x = _x;
+  y = _y;
+  z = _z;
+}
+
+//computes the angle between two vectors (they *MUST* first be normalized)
+double CPoint3::angleBetween(const CPoint3 &inPoint)
+{
+  double diffNorm, sumNorm;
+  sumNorm = (*this + inPoint).norm();
+  diffNorm = (*this - inPoint).norm();
+  return 2.0*atan2(diffNorm,sumNorm);
+}
+
+//makes a vector orthogonal to this.  
+CPoint3  CPoint3::constructOrthogonalVector(void)
+{
+  double thisLength, maxAbsValue;
+  CPoint3 returnVector, unitVector;
+  int maxAbsIndex;
+
+  maxAbsValue = maxabs(&maxAbsIndex);
+  thisLength = norm();
+  if(thisLength == 0.0)
+  {
+    returnVector[0] = 1.0;
+    return returnVector;
+  }
+
+  //try to construct a vector sufficiently different from this.
+  returnVector[maxAbsIndex] = 0.0;
+  returnVector[(maxAbsIndex + 1) % 3] = maxAbsValue;
+  returnVector[(maxAbsIndex + 2) % 3] = 0;
+  unitVector = *this;
+  unitVector.normalize();
+
+  //now we should have sufficient difference between returnPoint and this
+  //to do a gram-schmidt orthogonal construction
+  returnVector = returnVector - (returnVector*unitVector)*unitVector;
+  returnVector.normalize();
+  return returnVector;
+}
+
+void operator>>(CArchive &InArch, CPoint3 & InPnt)
+{
+  InPnt.Serialize(InArch);
+}
+
+void operator<<(CArchive &InArch, CPoint3 & InPnt)
+{
+  InPnt.Serialize(InArch);
+}
+
+BOOL CPoint3::toVARIANT(VARIANT *pv)
+{
+  BOOL            retval = FALSE;
+  SAFEARRAY       *tmp;
+  SAFEARRAYBOUND  a[1];
+  long i;
+
+  pv->vt = VT_ARRAY | VT_R8;
+
+  a[0].lLbound = 0;
+  a[0].cElements = 3;
+
+  tmp = SafeArrayCreate(VT_R8, 1, a);
+  if (tmp)
+  {
+    double m_data[3]={0.0};
+    m_data[0]=x;
+    m_data[1]=y;
+    m_data[2]=z;
+    for (i = 0; i < 3; i++)
+      SafeArrayPutElement(tmp, &i, &m_data[i]);
+
+    pv->parray = tmp;
+    retval = TRUE;
+  }
+
+  return retval;
+}
+
+BOOL CPoint3::fromVARIANT(VARIANT pv)
+{
+  BOOL      retval = FALSE;
+  SAFEARRAY *tmp;
+  long      num;
+  long      i;
+
+  if(pv.vt && (pv.vt & VT_ARRAY) && (pv.vt & VT_R8) && pv.parray)
+  {
+    tmp = pv.parray;
+    num = tmp->rgsabound->cElements;
+    if(num == 3)
+    {
+      double m_data[3]={0.0};
+      m_data[0]=x;
+      m_data[1]=y;
+      m_data[2]=z;
+      for (i = 0; i < num; i++)
+        SafeArrayGetElement(tmp, &i, &m_data[i]);
+      retval = TRUE;
+    }
+  }
+  return retval;
+}