mathex.cpp

常用计算机图形学算法.包括射线,球,直线,与轴平行包围盒相交计算,常用矩阵运算等.

#include "Angle.h"
#include "MathEx.h"
#include "TVector2.h"
#include "TVector3.h"
#include "Matrix3r.h"
#include "Matrix4r.h"
#include "Plane.h"
#include "Ray3D.h"
#include "Ray2D.h"
#include "Sphere.h"
#include "AxisAlignedBox.h"
#include "AxisAlignedRect.h"
#include "Quaternion.h"

#include <iostream>


    bool MathEx::Collinear(const Vector2r& a, const Vector2r& b, 
			               const Vector2r& c)
    {
        //设点为p,线段为ab,判断点Q在该直线上的依据是:(p-a)*(b-a)=0 
        return (c-a).Cross(b-a) == 0;
    }
    //-----------------------------------------------------------------------
    bool MathEx::Collinear(const Vector3r& a, const Vector3r& b, 
			               const Vector3r& c)
    {
        //链接原三点的三条向量满足op=λ*oa+(1-λ)*ob
        if( (c.x-a.x) * (b.y-a.y) - (c.y-a.y)*(b.x-a.x) == 0 &&
            (c.x-a.x) * (b.z-a.z) - (c.z-a.z)*(b.x-a.x) == 0 )
        {
            return true;
        }
        return false;
    }
    //-----------------------------------------------------------------------
    bool MathEx::PointOnSegment(const Vector2r& p, const Vector2r& a, 
			                 const Vector2r& b)
    {
        //先判断是否在直线上,再判断是否在两点之间

        if( !Collinear(p, a, b) )
        {
            return false;
        }
        if( Math::Min(a.x, b.x) <= p.x && p.x <= Math::Max(a.x, b.x) 
         && Math::Min(a.y, b.y) <= p.y && p.y <= Math::Max(a.y, b.y) )
        {
            return true;
        }
        return false;
    }
    //-----------------------------------------------------------------------
    bool MathEx::PointOnSegment(const Vector3r& p, const Vector3r& a, 
			                 const Vector3r& b)
    {
        //先判断是否在直线上,再判断是否在两点之间

        if( !Collinear(p, a, b) )
        {
            return false;
        }
        if( Math::Min(a.x, b.x) <= p.x && p.x <= Math::Max(a.x, b.x) 
         && Math::Min(a.y, b.y) <= p.y && p.y <= Math::Max(a.y, b.y)
         && Math::Min(a.z, b.z) <= p.z && p.z <= Math::Max(a.z, b.z) )
        {
            return true;
        }
        return false;
    }
    //-----------------------------------------------------------------------
    bool MathEx::PointInTri(const Vector2r& p, const Vector2r& a, 
			                const Vector2r& b, const Vector2r& c)
    {
        //p,c是否在ab同侧   
        //p,a是否在bc同侧   
        //p,b是否在ac同侧   

        //三个条件满足,则p点在a,b,c为顶点的三角形内.

        Vector2r v1, v2;
		Real dot[3];
		bool zeroDot[3];

		v1 = b - a;
		v2 = p - a;

		dot[0] = v1.Cross(v2);
		zeroDot[0] = Math::IsEqual(dot[0], 0.0f);


		v1 = c - b;
		v2 = p - b;

		dot[1] = v1.Cross(v2);
		zeroDot[1] = Math::IsEqual(dot[1], 0.0f);

		if(!zeroDot[0] && !zeroDot[1] 
		&& Math::Sign(dot[0]) != Math::Sign(dot[1]))
		{
			return false;
		}

		v1 = a - c;
		v2 = p - c;

		dot[2] = v1.Cross(v2);
		zeroDot[2] = Math::IsEqual(dot[2], 0.0f);

		if((!zeroDot[0] && !zeroDot[2] 
			&& Math::Sign(dot[0]) != Math::Sign(dot[2])) ||
			(!zeroDot[1] && !zeroDot[2] 
			&& Math::Sign(dot[1]) != Math::Sign(dot[2])))
		{
			return false;
		}
        return true;
    }
    //-----------------------------------------------------------------------
    bool MathEx::PointInTri(const Vector3r& p, const Vector3r& a, 
			                const Vector3r& b, const Vector3r& c, bool isKnowCoplanarity)
    {
        return PointInTri(p, a, b, c, FaceNormal(a, b, c), isKnowCoplanarity );
    }
    //-----------------------------------------------------------------------
    bool MathEx::PointInTri(const Vector3r& p, const Vector3r& a, 
			                const Vector3r& b, const Vector3r& c,
                            const Vector3r& normal, bool isKnowCoplanarity)
    {
        //p,c是否在ab同侧
        //p,a是否在bc同侧
        //p,b是否在ac同侧
        //p是否在abc的平面上

        //四个条件满足,则p点在a,b,c为顶点的三角形内.

        Vector3r v1, v2;
		Real dot[3];
		bool zeroDot[3];

		v1 = b - a;
		v2 = p - a;

        //先判断是否共面
        if(!isKnowCoplanarity)
        {
            if( !Math::IsEqual(v2.Dot(normal), 0.0f) )
            {
                return false;
            }
        }

		dot[0] = v1.Cross(v2).Dot(normal);
		zeroDot[0] = Math::IsEqual(dot[0], 0.0f);


		v1 = c - b;
		v2 = p - b;

		dot[1] = v1.Cross(v2).Dot(normal);
		zeroDot[1] = Math::IsEqual(dot[1], 0.0f);

		if(!zeroDot[0] && !zeroDot[1] 
		&& Math::Sign(dot[0]) != Math::Sign(dot[1]))
		{
			return false;
		}

		v1 = a - c;
		v2 = p - c;

		dot[2] = v1.Cross(v2).Dot(normal);
		zeroDot[2] = Math::IsEqual(dot[2], 0.0f);

		if((!zeroDot[0] && !zeroDot[2] 
			&& Math::Sign(dot[0]) != Math::Sign(dot[2])) ||
			(!zeroDot[1] && !zeroDot[2] 
			&& Math::Sign(dot[1]) != Math::Sign(dot[2])))
		{
			return false;
		}

        return true;
    }
    //-----------------------------------------------------------------------
    Vector3r MathEx::FaceNormal(const Vector3r& a, const Vector3r& b, const Vector3r& c)
    {
        return (a - c).Cross(b - c);
    }
    //-----------------------------------------------------------------------
    bool MathEx::Intersect(const Vector2r& a1, const Vector2r& b1, 
			              const Vector2r& a2, const Vector2r& b2)
    {
        //确定两线段相交分两步:
        //1.快速排斥实验:设以线段a1,b2为对角线的矩形为R,设以线段a2b2为对角线
        //的矩形为T,如果R和T不相交,显然两线段不会相交;
        //2.跨立试验:如果两线段相交,则两线段必然相互跨立对方.若a1b1跨立a2b2,
        //矢量(a1-a2)和(b1-a2)位于矢量(a2-b2) 的两侧,即
        //(a1-a2)×(a1-b1)*(a1-b1)×(a1-b2)>=0 同理,a2b2跨立a1b1
        //(a2-a1)×(a2-b2)*(a2-b2)×(a2-b1)>=0

        //1.快速排斥实验
        if( (Math::Max(a1.x, b1.x)   <   Math::Min(a2.x, b2.x))
         || (Math::Max(a2.x, b2.x)   <   Math::Min(a1.x, b1.x))
         || (Math::Max(a1.y, b1.y)   <   Math::Min(a2.y, b2.y))
         || (Math::Max(a2.y, b2.y)   <   Math::Min(a1.y, b1.y)) )
        {
            return false;
        }

        //2.跨立试验
        if( (a1-a2).Cross(a1-b1) * (a1-b1).Cross(a1-b2) >= 0
         && (a2-a1).Cross(a2-b2) * (a2-b2).Cross(a2-b1) >= 0 )
        {
            return true;
        }
        return false;

    }
    //-----------------------------------------------------------------------
    bool MathEx::Intersect(const Vector3r& a1, const Vector3r& b1, 
			               const Vector3r& a2, const Vector3r& b2)
    {
        //直线L1方程,可以写为
        //(x-a1.x)/(b1.x-a1.x)=(y-a1.y)/(b1.y-a1.y)=(z-a1.z)/(b1.z-a1.z)
        //直线L2方程,可以写为
        //(x-a2.x)/(b2.x-a2.x)=(y-a2.y)/(b2.y-a2.y)=(z-a2.z)/(b2.z-a2.z)
        //直线L1,L2相交,则行列式
        //|a2.x-a1.x  a2.y-a1.y   a2.z-a1.z|
        //|b1.x-a1.x  b1.y-a1.y   b1.z-a1.z| = 0
        //|b2.x-a2.x  b2.y-a2.y   b2.z-a2.z|

        //1.快速排斥实验
        if( (Math::Max(a1.x, b1.x)   <   Math::Min(a2.x, b2.x))
         || (Math::Max(a2.x, b2.x)   <   Math::Min(a1.x, b1.x))
         || (Math::Max(a1.y, b1.y)   <   Math::Min(a2.y, b2.y))
         || (Math::Max(a2.y, b2.y)   <   Math::Min(a1.y, b1.y))
         || (Math::Max(a1.z, b1.z)   <   Math::Min(a2.z, b2.z))
         || (Math::Max(a2.z, b2.z)   <   Math::Min(a1.z, b1.z)))
        {
            return false;
        }

        //2.矩阵检验
        Matrix3r m(a2.x-a1.x,  a2.y-a1.y,   a2.z-a1.z,
                   b1.x-a1.x,  b1.y-a1.y,   b1.z-a1.z,
                   b2.x-a2.x,  b2.y-a2.y,   b2.z-a2.z);

        return m.Determinant() == 0;
    }
    //-----------------------------------------------------------------------
    bool MathEx::Intersect(const Ray2D& ray, const Vector2r& a, const Vector2r& b)
    {
        const Vector2r& rayorig = ray.GetOrigin();
        const Vector2r& raydir = ray.GetDirection();
        //射线的快速排斥:
        //如果射线原点在线段左边并且方向向左
        if( rayorig.x < Math::Min(a.x, b.x) && raydir.x < 0 )
        {
            return false;
        }
        //如果射线原点在线段右边并且方向向右
        if( rayorig.x > Math::Max(a.x, b.x) && raydir.x > 0 )
        {
            return false;
        }
        //如果射线原点在线段下边并且方向向下
        if( rayorig.y < Math::Min(a.y, b.y) && raydir.y < 0 )
        {
            return false;
        }
        //如果射线原点在线段上边并且方向向上
        if( rayorig.y > Math::Max(a.y, b.y) && raydir.y > 0 )
        {
            return false;
        }

        //如果直线ray与线段ab不相交
        if( (a - rayorig).Cross(raydir) * (b - rayorig).Cross(raydir) > 0 )
        {
            return false;
        }
        //在射线ray上取无穷远处一点rayp
        Vector2r rayp = ray.GetPoint( std::numeric_limits<Real>::max() );
        //如果rayp,rayorig线段与ab线段相交,则射线与线段也相交
        if( Intersect(rayorig, rayp, a, b) )
        {
            return true;
        }
        return false;
    }
    //-----------------------------------------------------------------------
    std::pair<RayPlaneRelation, Real> MathEx::Intersect(const Ray3D& ray, const Plane& plane,
                                                         bool positiveSide, bool negativeSide)
    {
        //射线 p(t)=p0+t*u 和平面 n*p+d=0,将射线代入平面方程
        //并求出满足平面方程的参数t,既得相交点.
        //t=(-d-n*p0)/(n*u),若t不在[0,*),则不相交,若在,则t代入射线
        //方程得到交点
        
        //求平面法线与射线方向的角度,(n*u)部分
        Real denom = plane.GetNormal().Dot( ray.GetDirection() );
        
        //检查相交面,相交于正面
        if(denom > + std::numeric_limits<Real>::epsilon())
        {
            if (!negativeSide)
            {
                return std::pair<RayPlaneRelation, Real>(RPR_NONINTERSECT, 0);
            }
        }
        //检查相交面,相交于背面
        else if (denom < - std::numeric_limits<Real>::epsilon())
        {
            if (!positiveSide)
            {
                return std::pair<RayPlaneRelation, Real>(RPR_NONINTERSECT, 0);
            }
        }

        if ( Math::IsEqual(denom, 0.0f) )
        {
            //如果平行
            if( plane.IsPointOn( ray.GetOrigin() ) )//如果射线在平面上
            {
                return std::pair<RayPlaneRelation, Real>(RPR_COPLANARITY, 0);
            }
            return std::pair<RayPlaneRelation, Real>(RPR_NONINTERSECT, 0);
        }
        else
        {
            //求(-d-n*p0)部分
            Real nom = plane.GetNormal().Dot(ray.GetOrigin()) + plane.Get_d();
            Real t = -(nom/denom);
            if(t >= 0)
            {
                return std::pair<RayPlaneRelation, Real>(RPR_INTERSECT, t);
            }
            else
            {
                return std::pair<RayPlaneRelation, Real>(RPR_NONINTERSECT, t);
            }
        }
    }
    //-----------------------------------------------------------------------
    std::pair<bool, Real> MathEx::Intersect(const Ray3D& ray, const Sphere& sphere)
    {
        //将射线方程 p(t)=p0+t*u 代入球方程 |p-c|=r 得