#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;
}