math.h

这是VCF框架的代码

			-x*q.z+y*q.w+z*q.x+w*q.y,
			+x*q.y-y*q.x+z*q.w+w*q.z,
			-x*q.x-y*q.y-z*q.z+w*q.w);        
	}

	inline Quaternion operator+(const Quaternion &q) const{
		return Quaternion(x+q.x,y+q.y,z+q.z,w+q.w);
	}

	inline void operator+=(const Quaternion &q){
		x+=q.x;
		y+=q.y;
		z+=q.z;
		w+=q.w;
	}

	inline Quaternion operator-(const Quaternion &q) const{
		return Quaternion(x-q.x,y-q.y,z-q.z,w-q.w);
	}

	inline void operator-=(const Quaternion &q){
		x-=q.x;
		y-=q.y;
		z-=q.z;
		w-=q.w;
	}

	inline Quaternion operator*(vrReal f) const{
		return Quaternion(x*f,y*f,z*f,w*f);
	}

	inline Quaternion operator/(vrReal f) const{
		f=1/f;
		return Quaternion(x*f,y*f,z*f,w*f);
	}

	inline vrReal &operator[](int i){
		return *(&x+i);
	}

	inline vrReal operator[](int i) const{
		return *(&x+i);
	}
};

inline void setQuaternion(Quaternion &quat,const Matrix3x3 &mat){
	vrReal w=0.5*(vrReal)sqrt(mat.at(0,0)+mat.at(1,1)+mat.at(2,2)+1);
	vrReal s=0.25/w;
	quat[0]=(mat.at(1,2)-mat.at(2,1))*s;
	quat[1]=(mat.at(2,0)-mat.at(0,2))*s;
	quat[2]=(mat.at(0,1)-mat.at(1,0))*s;
	quat[3]=w;
}

inline Quaternion makeQuaternion(const Matrix3x3 &mat){
	Quaternion quat;
	setQuaternion(quat,mat);
	return quat;
}

inline void setMatrix3x3(Matrix3x3 &mat,const Quaternion &quat){
	//WARNING: STATIC VARS
	static vrReal xx;
	static vrReal xy;
	static vrReal yy;
	static vrReal xz;
	static vrReal yz;
	static vrReal zz;
	static vrReal xw;
	static vrReal yw;
	static vrReal zw;
	static vrReal ww;

	xx=quat.x*quat.x;
	xy=quat.x*quat.y;
	yy=quat.y*quat.y;
	xz=quat.x*quat.z;
	yz=quat.y*quat.z;
	zz=quat.z*quat.z;
	xw=quat.x*quat.w;
	yw=quat.y*quat.w;
	zw=quat.z*quat.w;
	ww=quat.w*quat.w;

	mat=Matrix3x3(
		+xx-yy-zz+ww, +xy-zw+xy-zw, +xz+yw+xz+yw,
		+xy+zw+xy+zw, -xx+yy-zz+ww, +yz-xw+yz-xw,
		+xz-yw+xz-yw, +yz+xw+yz+xw, -xx-yy+zz+ww);
}

inline Matrix3x3 makeMatrix3x3(const Quaternion &quat){
	Matrix3x3 mat;
	setMatrix3x3(mat,quat);
	return mat;
}

inline vrReal norm(const Quaternion &quat){
	return quat.x*quat.x+quat.y*quat.y+quat.z*quat.z+quat.w*quat.w;
}

inline vrReal length(const Quaternion &quat){
	return sqrt(norm(quat));
}

inline Quaternion normalize(Quaternion &quat){
	vrReal l=1/length(quat);
	quat.x*=l;
	quat.y*=l;
	quat.z*=l;
	quat.w*=l;
	return quat;
}

inline Quaternion normalize(const Quaternion &quat){
	vrReal l=1/length(quat);
	Quaternion quat2;
	quat2.x=quat.x*l;
	quat2.y=quat.y*l;
	quat2.z=quat.z*l;
	quat2.w=quat.w*l;
	return quat2;
}

inline Quaternion makeConjugate(const Quaternion &quat){
	return Quaternion(-quat.x,-quat.y,-quat.z,-quat.w);
}

inline void setInverse(Quaternion &res,const Quaternion &src){
	res=makeConjugate(src)/norm(src);
}

inline Quaternion makeInverse(const Quaternion &src){
	Quaternion quat;
	setInverse(quat,src);
	return quat;
}

inline Quaternion makeExp(const Quaternion &quat){
	vrReal r  = sqrt(quat.x*quat.x+quat.y*quat.y+quat.z*quat.z);
	vrReal et = exp(quat.w);
	vrReal s  = r>=0.00001f? et*sin(r)/r: 0.f;
	return Quaternion(s*quat.x,s*quat.y,s*quat.z,et*cos(r));
}

inline Quaternion makeLn(const Quaternion &quat){
	vrReal r  = sqrt(quat.x*quat.x+quat.y*quat.y+quat.z*quat.z);
	vrReal t  = r>0.00001f? atan2(r,quat.w)/r: 0.f;
	return Quaternion(t*quat.x,t*quat.y,t*quat.z,0.5*log(norm(quat)));
}

inline vrReal dot(const Quaternion &quat1,const Quaternion &quat2){
	return quat1.x*quat2.x+quat1.y*quat2.y+quat1.z*quat2.z+quat1.w*quat2.w;
}

inline Quaternion slerp(const Quaternion &quat1,const Quaternion &quat2,vrReal t){
	Quaternion l1;
	Quaternion l2;
		
	l1=makeLn(quat1);

	if(dot(quat1,quat2)<0)
		l2=makeLn(quat2*(-1));
	else
		l2=makeLn(quat2);

	Quaternion r=linearInterpolate(l1,l2,t);

	return makeExp(r);
}

inline Quaternion fastSlerp(const Quaternion &quat1,const Quaternion &quat2,vrReal t){
	if(dot(quat1,quat2)<0)
		return linearInterpolate(quat1,quat2*(-1),t);
	else
		return linearInterpolate(quat1,quat2,t);
}

inline Quaternion makeQuaternionFromVectorToVector(const Vector3 &vec1,const Vector3 &vec2){
	Vector3 axis=normalize(cross(vec1,vec2));
	vrReal angle=acos(dot(vec1,vec2));
	Vector4 angleAxis(axis,angle);

	// Now make a Quaternion from AngleAxis
	vrReal s = (vrReal)sin(angleAxis.w/2.0);
	vrReal c = (vrReal)cos(angleAxis.w/2.0);
	return normalize(Quaternion(angleAxis.x*s,angleAxis.y*s,angleAxis.z*s,c));
}

// Segment
class Segment{
public:
	inline Segment(const Vector3 &paramStart,const Vector3 &paramEnd):origin(paramStart),direction(paramEnd-paramStart){}

	static inline Segment makeSegmentStartDir(const Vector3 &paramStart,const Vector3 &paramDir){return Segment(paramStart,paramStart+paramDir);}

	inline const Vector3& getOrigin() const{
		return origin;
	}

	inline Vector3& Origin(){
		return origin;
	}

	inline const Vector3& getDirection() const{
		return direction;
	}
	
	inline Vector3& Direction(){
		return direction;
	}

	inline Vector3 getEndPoint() const{
		return origin+direction;
	}

private:
	Vector3 origin;
	Vector3 direction;
};

// Plane
class Plane;
Plane normalize(Plane &plane);

class Plane{
public:
	inline Plane(){}

	inline Plane(const Vector3 &norm,vrReal dist):normal(norm),d(dist){
	}

	inline Plane(const Vector3 &point,const Vector3 &norm){
		normal=norm;
		d=dot(normal,point);
	}

	inline Plane(const Vector3 &point1,const Vector3 &point2,const Vector3 &point3){
		Vector3 v1,v2;

		v1=point3-point1;
		v2=point2-point1;

		normal=cross(v2,v1);

		d=dot(normal,point1);

		normalize(*this);
	}

	Segment getLineOfIntersection(const Plane &plane2){
		vrReal determinant=lengthSquared(normal) * lengthSquared(plane2.normal) - (dot(normal,plane2.normal)*dot(normal,plane2.normal));

		vrReal c1=(d*lengthSquared(plane2.normal) - plane2.d*dot(normal,plane2.normal))/determinant;
		vrReal c2=(plane2.d*lengthSquared(normal) - d*dot(normal,plane2.normal))/determinant;

		Vector3 dir=cross(normal,plane2.normal);
		Vector3 orig=c1*normal+c2*plane2.normal;

		return Segment::makeSegmentStartDir(orig,dir);
	}

	inline bool isPointOutside(const Vector3 &point) const{
		return normal.x*point.x+normal.y*point.y+normal.z*point.z-d>0;//(normal.dot(point)-d>0);
	}

	inline bool traceLine(const Vector3 &start,const Vector3 &end,Vector3 &result,bool infinite) const{
		vrReal t=(dot(normal,normal*d-start))/(dot(normal,end-start));

		result=linearInterpolate(start,end,t);

		return true;
	}

	inline bool traceLine(const Vector3 &start,const Vector3 &end,Vector3 &result) const{
		vrReal t=(dot(normal,normal*d-start))/(dot(normal,end-start));

		if(t<0 || t>1)
			return false;

		result=linearInterpolate(start,end,t);

		return true;
	}

	inline bool traceLine(const Segment &seg,Vector3 &result) const{
		vrReal t=(dot(normal,normal*d-seg.getOrigin()))/(dot(normal,seg.getDirection()));

		if(t<0)
			return false;

		result=linearInterpolate(seg.getOrigin(),seg.getEndPoint(),t);

		return true;
	}

	inline Vector3 projectPoint(const Vector3 &point) const{
		Vector3 t=point-normal*d;

		vrReal f=dot(normal,t);

		return point-normal*f;
	}

	static bool getIntersectionOfThreePlanes(const Plane &p1,const Plane &p2,const Plane &p3,Vector3 &result){
		// Either one of these should work, but the second is quicker, no inverse
#if 0
		Matrix4x4 m,m2;

		m.at(0,0)=p1.normal.x;
		m.at(1,0)=p1.normal.y;
		m.at(2,0)=p1.normal.z;
		m.at(3,0)=p1.d;

		m.at(0,1)=p2.normal.x;
		m.at(1,1)=p2.normal.y;
		m.at(2,1)=p2.normal.z;
		m.at(3,1)=p2.d;

		m.at(0,2)=p3.normal.x;
		m.at(1,2)=p3.normal.y;
		m.at(2,2)=p3.normal.z;
		m.at(3,2)=p3.d;

		m.at(0,3)=0;
		m.at(1,3)=0;
		m.at(2,3)=0;
		m.at(3,3)=1;

		if(m.getInverse(m2)==false){
			return false;
		}
		else{
			result=Vector3(m2.at(3,0),m2.at(3,1),m2.at(3,2))*-1;
			return true;
		}
#else
		float den=dot(p1.normal,cross(p2.normal,p3.normal));
		if(den==0){
			return false;
		}
		else{
			result=-1*(-p1.d*cross(p2.normal,p3.normal) - p2.d*cross(p3.normal,p1.normal) - p3.d*cross(p1.normal,p2.normal))/den;
			return true;
		}
#endif
	}

	Vector3 normal;
	vrReal d;
};

inline Plane normalize(Plane &plane){
	vrReal l=1/length(plane.normal);
	plane.normal.x*=l;
	plane.normal.y*=l;
	plane.normal.z*=l;
	plane.d*=l;
	return plane;
}

inline Plane normalize(const Plane &plane){
	Plane plane2;
	vrReal l=1/length(plane.normal);
	plane2.normal.x=plane.normal.x*l;
	plane2.normal.y=plane.normal.y*l;
	plane2.normal.z=plane.normal.z*l;
	plane2.d=plane.d*l;
	return plane2;
}

//AABox
class AABox{
public:
	inline AABox(){}

	inline AABox(const Vector3 &paramMins,const Vector3 &paramMaxs):mins(paramMins),maxs(paramMaxs){}

	inline Vector3& Mins(){
		return mins;
	}

	inline Vector3 getMins() const{
		return mins;
	}

	inline Vector3& Maxs(){
		return maxs;
	}

	inline Vector3 getMaxs() const{
		return maxs;
	}

	inline void setMins(const Vector3 &min){
		mins=min;
	}

	inline void setMaxs(const Vector3 &max){
		maxs=max;
	}

	inline void getVertexes(Vector3 *vertexes) const{
		vertexes[0]=Vector3(mins.x,mins.y,mins.z);
		vertexes[1]=Vector3(maxs.x,mins.y,mins.z);
		vertexes[2]=Vector3(mins.x,maxs.y,mins.z);
		vertexes[3]=Vector3(maxs.x,maxs.y,mins.z);
		vertexes[4]=Vector3(mins.x,mins.y,maxs.z);
		vertexes[5]=Vector3(maxs.x,mins.y,maxs.z);
		vertexes[6]=Vector3(mins.x,maxs.y,maxs.z);
		vertexes[7]=Vector3(maxs.x,maxs.y,maxs.z);
	}

	inline void mergeWith(const AABox &box){
		if(box.mins.x<mins.x)mins.x=box.mins.x;
		if(box.mins.y<mins.y)mins.y=box.mins.y;
		if(box.mins.z<mins.z)mins.z=box.mins.z;
		if(box.maxs.x>maxs.x)maxs.x=box.maxs.x;
		if(box.maxs.y>maxs.y)maxs.y=box.maxs.y;
		if(box.maxs.z>maxs.z)maxs.z=box.maxs.z;
	}

	inline void mergeWith(const Vector3 &vec){
		if(vec.x<mins.x)mins.x=vec.x;
		if(vec.y<mins.y)mins.y=vec.y;
		if(vec.z<mins.z)mins.z=vec.z;
		if(vec.x>maxs.x)maxs.x=vec.x;
		if(vec.y>maxs.y)maxs.y=vec.y;
		if(vec.z>maxs.z)maxs.z=vec.z;
	}

	void rotate(const Matrix3x3 &rotation){
		int i;
		Vector3 vertexes[8];

		getVertexes(vertexes);

		for(i=0;i<8;i++){
			vertexes[i]=rotation*vertexes[i];
		}

		mins=vertexes[0];
		maxs=vertexes[0];

		for(i=1;i<8;i++){
			if(mins.x>vertexes[i].x)
				mins.x=vertexes[i].x;
			if(mins.y>vertexes[i].y)
				mins.y=vertexes[i].y;
			if(mins.z>vertexes[i].z)
				mins.z=vertexes[i].z;
			if(maxs.x<vertexes[i].x)
				maxs.x=vertexes[i].x;
			if(maxs.y<vertexes[i].y)
				maxs.y=vertexes[i].y;
			if(maxs.z<vertexes[i].z)
				maxs.z=vertexes[i].z;
		}
	}

	inline bool operator!=(const AABox &box) const{
		return (box.mins!=mins || box.maxs!=maxs);
	}

	inline AABox operator+(const Vector3 &vec) const{
		return AABox(mins+vec,maxs+vec);
	}

	inline void operator+=(const Vector3 &vec){
		mins+=vec;
		maxs+=vec;
	}

	inline AABox operator-(const Vector3 &vec) const{
		return AABox(mins-vec,maxs-vec);
	}

	inline void operator-=(const Vector3 &vec){
		mins-=vec;
		maxs-=vec;
	}

	inline AABox operator*(const Vector3 &vec) const{
		return AABox(mins*vec,maxs*vec);
	}

	inline AABox operator/(const Vector3 &vec) const{
		return AABox(mins/vec,maxs/vec);
	}

	//Scalar operations
	inline void operator*=(vrReal f){
		mins*=f;
		maxs*=f;
	}

	inline void operator/=(vrReal f){
		mins/=f;
		maxs/=f;
	}

	Vector3 mins,maxs;
};

//Sphere
class Sphere{
public:
	inline Sphere():radius(0){}
	inline Sphere(const Vector3 &paramOrigin,vrReal paramRadius):origin(paramOrigin),radius(paramRadius){}

	inline Vector3& getOrigin(){
		return origin;
	}

	inline const Vector3& getOrigin() const{
		return origin;
	}

	inline vrReal getRadius() const{
		return radius;
	}

	Vector3 origin;
	vrReal radius;
};

//Miscellanious
AABox calculateTightBox(const std::vector<Vector3> &vertexes);
AABox calculateLooseBox(const std::vector<Vector3> &vertexes);
Sphere makeBoundingSphere(AABox &box);

inline bool fuzzyEqual(vrReal a,vrReal b,vrReal epsilon){
	return(abs(a-b)<epsilon);
}

inline bool fuzzyEqual(const Vector3 &a,const Vector3 &b,vrReal epsilon){
	return(fuzzyEqual(a.x,b.x,epsilon) && fuzzyEqual(a.y,b.y,epsilon) && fuzzyEqual(a.z,b.z,epsilon));
}

}

#endif