vector3.cpp

机器人运动学动力学及控制的一个小程序,供有兴趣的人参考

// Vector3.cpp: implementation of the CVector3 class.
//
//////////////////////////////////////////////////////////////////////


#include "stdafx.h"
#include "Vector3.h"
#include<math.h>

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CVector3::CVector3()
{
  x=0;y=0;z=0;
}

CVector3::CVector3(double v)
{ x=v;y=v;z=v;

	}

CVector3::CVector3(double v1,double v2,double v3)
{
	x=v1;y=v2;z=v3;
}

CVector3::~CVector3()
{

}

//  下面的这些函数主要用来计算顶点的法向量，顶点的法向量主要用来计算光照

// 下面的函数求两点决定的矢量
CVector3& CVector3::Vector(CVector3 vPoint1, CVector3 vPoint2)
{
//	CVector3 vVector;							

	x = vPoint1.x - vPoint2.x;			
	y = vPoint1.y - vPoint2.y;			
	z = vPoint1.z - vPoint2.z;			

	return *this;								
}

// 下面的函数两个矢量相加
CVector3& CVector3::AddVector(CVector3 vVector1, CVector3 vVector2)
{
//	CVector3 vResult;							
	
	x = vVector2.x + vVector1.x;		
	y = vVector2.y + vVector1.y;		
	z = vVector2.z + vVector1.z;		

	return *this;
//	return vResult;								
}

// 下面的函数处理矢量的缩放
CVector3& CVector3::DivideVectorByScaler(CVector3 vVector1, double Scaler)
{
//	CVector3 vResult;							
	
	x = vVector1.x / Scaler;			
	y = vVector1.y / Scaler;			
	z = vVector1.z / Scaler;			

     return *this;
//	return vResult;								
}

CVector3& CVector3::MultiplyVectorByScaler(CVector3 vVector1, double Scaler)
{//CVector3 vResult;
            x = vVector1.x * Scaler;			
	        y = vVector1.y * Scaler;			
	        z = vVector1.z * Scaler;			

   return *this;	
}


// 下面的函数返回两个矢量的叉积
CVector3& CVector3::Cross(CVector3 vVector1, CVector3 vVector2)
{
//	CVector3 vCross;								
												
	x = ((vVector1.y * vVector2.z) - (vVector1.z * vVector2.y));
												
	y = ((vVector1.z * vVector2.x) - (vVector1.x * vVector2.z));
												
	z = ((vVector1.x * vVector2.y) - (vVector1.y * vVector2.x));

	return *this;
//	return vCross;								
}

// 下面的函数返回两个矢量的点积
double CVector3::Multiply(CVector3 vVector1, CVector3 vVector2)
{
	return vVector1.x*vVector2.x+vVector1.y*vVector2.y+vVector1.z*vVector2.z;
	
}

// 下面的函数规范化矢量
CVector3& CVector3::Normalize(CVector3 vNormal)
{
	double Magnitude;							

	Magnitude = Mag(vNormal);					// 获得矢量的长度
	if(Magnitude!=0){
	x /= (float)Magnitude;				
	y /= (float)Magnitude;				
	z /= (float)Magnitude;	
	}
else
{
	x=0;
	y=0;
	z=0;
}
	return *this;
//	return vNormal;								
}
/*
CMatrix CVector3::ToMatrix41(int point)
{   double v[4]={x,y,z,point};
    CMatrix result(4,1,v);
    return result;
}
*/
CVector3& CVector3::operator=(const CVector3& c)
{//   if(&c!=this)
//CVector3 result(*this);
	x=c.x;
    y=c.y;
    z=c.z;
     return *this;
}

CVector3 CVector3::operator+(const CVector3 &v)const
{ CVector3 result(*this);
          result.AddVector(*this,v);
			  
		  return result;
}


CVector3 CVector3::operator-(const CVector3 &v)const
{     CVector3 result(*this);
          result.Vector(*this,v);
			  
		  return result;
}

double CVector3::operator*(const CVector3 v)const
{//CVector3 result(*this);
   return  x*v.x+y*v.y+z*v.z;
}

CVector3 CVector3::operator*(const double &scale)const
{  CVector3 result(*this);
    result.x=x*scale;
	result.y=y*scale;
	result.z=z*scale;
	return result;
}

CVector3 CVector3::operator^(const CVector3 v)const
{
   CVector3 result(*this);
          result.Cross(*this,v);
			  
		  return result;
}

CVector3 CVector3::operator/(const double scale)const
{
CVector3 result(*this);
    result.x=x/scale;
	result.y=y/scale;
	result.z=z/scale;
	return result;
}

CVector3 CVector3::operator-()const
{CVector3 result(*this);
    result.x=-x;
	result.y=-y;
	result.z=-z;
	return result;
}

BOOL CVector3::operator==(const CVector3 &v) const
{ if(fabs(x-v.x)>1e-8||fabs(y-v.y)>1e-8||fabs(z-v.z)>1e-8) return false;
   return true;
}

BOOL CVector3::operator!=(const CVector3 &v) const
{	return !(*this == v);
}