quaternion.cpp

来自「3D游戏展示程序」· C++ 代码 · 共 135 行

//--------------------------------------------------
//  Desc: quaternion
//  Author: artsylee/2007.4.2
//--------------------------------------------------

#include "Quaternion.h"
#include <math.h>

Vector4D::Vector4D()
{
	x = 0.0f;
	y = 0.0f;
	z = 0.0f;
	w = 0.0f;
}

Vector4D::Vector4D(const Vector4D &v)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = v.w;
}

Vector4D::Vector4D(float fx, float fy, float fz, float fw)
{ 
	x = fx; 
	y = fy; 
	z = fz;	
	w = fw;
}

Vector4D::Vector4D(const Vector3D &v, const float fw)
{
	x = v.x;
	y = v.y;
	z = v.z;
	w = fw;
}

Vector4D Vector4D::operator+(const Vector4D &v) const
{ 
	return Vector4D(x + v.x, y + v.y, z + v.z, w + v.w);	
}

Vector4D Vector4D::operator-(const Vector4D &v) const
{ 
	return Vector4D(x - v.x, y - v.y, z - v.z, w - v.w);	
}

Vector4D Vector4D::operator*(float scale) const
{ 
	return Vector4D(x*scale, y*scale, z*scale, w*scale);	
}

float Vector4D::operator*(const Vector4D &v) const
{
	return x*v.x + y*v.y + z*v.z + w*v.w;
}

Vector4D& Vector4D::operator=(const Vector4D &v)
{ 
	x = v.x; 
	y = v.y; 
	z = v.z; 
	w = v.w;
	return *this;	
}

Vector4D& Vector4D::operator+=(const Vector4D &v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	w += v.w;
	return *this;
}

Vector4D& Vector4D::operator-=(const Vector4D &v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	w -= v.w;
	return *this;
}

 Vector4D& Vector4D::operator*=(float scale)
{
	x *= scale;
	y *= scale;
	z *= scale;
	w *= scale;
	return *this;
}

float Vector4D::LengthSquared() const
{
	return x*x + y*y + z*z + w*w;
}

float Vector4D::Length() const
{
	return sqrtf(x*x + y*y + z*z + w*w);
}

Vector4D& Vector4D::Normalize()
{
	this->operator *= (1.0f/Length());
	return *this;
}

//--------------------------------------------------
// QUTERNION
//--------------------------------------------------
Quaternion Lerp(const float r, const Quaternion &v1, const Quaternion &v2)
{
	return v1*(1.0f-r) + v2*r;
}

Quaternion Slerp(const float r, const Quaternion &v1, const Quaternion &v2)
{
	float dot = v1 * v2;
	if(fabs(dot) > 0.9995f) 
	{
		return Lerp(r, v1, v2);
	}

	float a = acosf(dot) * r;
	Quaternion q = (v2 - v1 * dot);
	q.Normalize();

	return v1 * cosf(a) + q * sinf(a);
}