Box2d源码学习<四>数学库API的实现
本系列博客是由扭曲45原创，欢迎转载，转载时注明出处，http://blog.csdn.net/cg0206/article/details/8275826
为了满足Box2d中所要用到的数学知识，在Box2d的公共模块中，包含了一个小巧而简便的向量矩阵的数学库。这部分所有的定义实现均有结构体实现的，所以其内部的成员均对外部公开，所以你可以任意使用和访问。该部分主要由以下内容：
a)、向量，包括二维列向量和三维列向量
b)、矩阵，包括2X2矩阵和3X3矩阵
c)、旋度、扫描、和变换的实现
d)、其他部分的实现
一、向量部分
1、 二维列向量的实现
//2d列向量
struct b2Vec2
{
/**************************************************************************
* 功能描述：构造函数，不做任何事，仅仅为了性能
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Vec2() {}
/**************************************************************************
* 功能描述：构造函数，构建使用的坐标
* 参数说明： x:x坐标
y:y坐标
* 返 回 值： (void)
**************************************************************************/
b2Vec2(float32 x, float32 y) : x(x), y(y) {}
/**************************************************************************
* 功能描述：将向量置0
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetZero() { x = 0.0f; y = 0.0f; }
/**************************************************************************
* 功能描述：将向量设置成指定的坐标
* 参数说明： x_:x坐标
y_:y坐标
* 返 回 值： (void)
**************************************************************************/
void Set(float32 x_, float32 y_) { x = x_; y = y_; }
/**************************************************************************
* 功能描述：将向量取反，重载 - 运算符
* 参数说明： (void)
* 返 回 值： 一个新的二维向量
**************************************************************************/
b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
/**************************************************************************
* 功能描述：读取指定索引的元素，重载 () 运算符
* 参数说明： i ：索引值
* 返 回 值： 列中对应索引元素值
**************************************************************************/
float32 operator () (int32 i) const
{
return (&x)[i];
}
/**************************************************************************
* 功能描述：改写指定索引的元素，重载 () 运算符
* 参数说明： i ：索引值
* 返 回 值： 列中对应索引元素的引用
**************************************************************************/
float32& operator () (int32 i)
{
return (&x)[i];
}
/**************************************************************************
* 功能描述：与另一个向量相加
* 参数说明： v ：向量的引用
* 返 回 值： (void)
**************************************************************************/
void operator += (const b2Vec2& v)
{
x += v.x; y += v.y;
}
/**************************************************************************
* 功能描述：与另一个向量相减
* 参数说明： v ：向量的引用
* 返 回 值： (void)
**************************************************************************/
void operator -= (const b2Vec2& v)
{
x -= v.x; y -= v.y;
}
/**************************************************************************
* 功能描述：与标量相乘
* 参数说明： a ：标量
* 返 回 值： (void)
**************************************************************************/
void operator *= (float32 a)
{
x *= a; y *= a;
}
/**************************************************************************
* 功能描述：获取向量的长度
* 参数说明： (void)
* 返 回 值： 向量长度
**************************************************************************/
float32 Length() const
{
return b2Sqrt(x * x + y * y);
}
/**************************************************************************
* 功能描述：获取向量的长度的平方。出于性能要求，
使用此函数代替b2Vec2::Legth函数（如果可能的话）
* 参数说明： (void)
* 返 回 值： 向量长度的平方
**************************************************************************/
float32 LengthSquared() const
{
return x * x + y * y;
}
/**************************************************************************
* 功能描述：将该向量转变成单位向量
* 参数说明： (void)
* 返 回 值： 向量长度[注意是未转变之前的长度]
**************************************************************************/
float32 Normalize()
{
//获取该向量的长度并验证其有效性
float32 length = Length();
if (length < b2_epsilon)
{
return 0.0f;
}
//获取要转换单位向量的因子,并与x、y轴相乘
float32 invLength = 1.0f / length;
x *= invLength;
y *= invLength;
return length;
}
/**************************************************************************
* 功能描述：该向量包含的坐标值是否有效
* 参数说明： (void)
* 返 回 值： 是否有效
**************************************************************************/
bool IsValid() const
{
return b2IsValid(x) && b2IsValid(y);
}
/**************************************************************************
* 功能描述：获取斜对称向量(反对称向量),dot(skew_vec, other) == cross(vec, other)
* 参数说明： (void)
* 返 回 值： 斜对称向量(反对称向量)
**************************************************************************/
b2Vec2 Skew() const
{
return b2Vec2(-y, x);
}
//向量元素的x，y值
float32 x, y;
};
2、 三维列向量的实现
//2d三维列向量
struct b2Vec3
{
/**************************************************************************
* 功能描述：默认构造函数，不做任何事，仅仅为了性能
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Vec3() {}
/**************************************************************************
* 功能描述：构造函数，构建使用的坐标
* 参数说明： x:x坐标
y:y坐标
* 返 回 值： (void)
**************************************************************************/
b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
/**************************************************************************
* 功能描述：将向量置0
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
/**************************************************************************
* 功能描述：将向量设置成指定的坐标
* 参数说明： x_:x坐标
y_:y坐标
x_:z坐标
* 返 回 值： (void)
**************************************************************************/
void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
/**************************************************************************
* 功能描述：将向量取反，重载 - 运算符
* 参数说明： (void)
* 返 回 值： 一个新的三维向量
**************************************************************************/
b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
/**************************************************************************
* 功能描述：与另一个向量相加
* 参数说明： v ：向量的引用
* 返 回 值： (void)
**************************************************************************/
void operator += (const b2Vec3& v)
{
x += v.x; y += v.y; z += v.z;
}
/**************************************************************************
* 功能描述：与另一个向量相减
* 参数说明： v ：向量的引用
* 返 回 值： (void)
**************************************************************************/
void operator -= (const b2Vec3& v)
{
x -= v.x; y -= v.y; z -= v.z;
}
/**************************************************************************
* 功能描述：与一个标量相乘
* 参数说明： s :标量
* 返 回 值： (void)
**************************************************************************/
void operator *= (float32 s)
{
x *= s; y *= s; z *= s;
}
//向量的元素值
float32 x, y, z;
};
二、 矩阵
1、2X2矩阵的实现
//2X2矩阵，存储方式以列为主的顺序
struct b2Mat22
{
/**************************************************************************
* 功能描述：默认构造函数不做任何事情(仅仅为了性能)
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Mat22() {}
/**************************************************************************
* 功能描述：使用列向量构造这个矩阵
* 参数说明： c1 ：第一列向量
c2 ：第二列向量
* 返 回 值： (void)
**************************************************************************/
b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
{
ex = c1;
ey = c2;
}
/**************************************************************************
* 功能描述：使用标量构造这个矩阵
* 参数说明： a11-a22:四个元素值
* 返 回 值： (void)
**************************************************************************/
b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
{
ex.x = a11; ex.y = a21;
ey.x = a12; ey.y = a22;
}
/**************************************************************************
* 功能描述：使用列实例话这个矩阵
* 参数说明： c1:列向量
c2:列向量
* 返 回 值： (void)
**************************************************************************/
void Set(const b2Vec2& c1, const b2Vec2& c2)
{
ex = c1;
ey = c2;
}
/**************************************************************************
* 功能描述：设置成单位矩阵
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetIdentity()
{
ex.x = 1.0f; ey.x = 0.0f;
ex.y = 0.0f; ey.y = 1.0f;
}
/**************************************************************************
* 功能描述：将该矩阵置0
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetZero()
{
ex.x = 0.0f; ey.x = 0.0f;
ex.y = 0.0f; ey.y = 0.0f;
}
/**************************************************************************
* 功能描述：获取逆矩阵,
逆矩阵B = A的伴随矩阵/A的行列式
参照 http://zh.wikipedia.org/wiki/逆矩阵
* 参数说明： (void)
* 返 回 值： 逆矩阵
**************************************************************************/
b2Mat22 GetInverse() const
{
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
b2Mat22 B;
float32 det = a * d - b * c; //获取行列式的值
if (det != 0.0f)
{
det = 1.0f / det;
}
B.ex.x = det * d; B.ey.x = -det * b;
B.ex.y = -det * c; B.ey.y = det * a;
return B;
}
/**************************************************************************
* 功能描述：解决A*x = b,其中b是一个列向量
这比一次性求反计算更有效率
* 参数说明： b : 列向量
* 返 回 值： 列向量
**************************************************************************/
b2Vec2 Solve(const b2Vec2& b) const
{
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
//获取行列式的值
float32 det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec2 x;
//[ a22 -a12] * [b.x] = [a22 * b.x - a12*b.y]
//[-a21 a11] [b.y] [a11 * b.y - a21*b.x]
x.x = det * (a22 * b.x - a12 * b.y);
x.y = det * (a11 * b.y - a21 * b.x);
return x;
}
//两个列向量
b2Vec2 ex, ey;
};
2、3X3矩阵的实现
//3X3矩阵，存储方式以列为主的顺序
struct b2Mat33
{
/**************************************************************************
* 功能描述：默认构造函数不做任何事情(仅仅为了性能)
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Mat33() {}
/**************************************************************************
* 功能描述：使用列向量构造这个矩阵
* 参数说明： c1 ：第一列向量
c2 ：第二列向量
* 返 回 值： (void)
**************************************************************************/
b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
{
ex = c1;
ey = c2;
ez = c3;
}
/**************************************************************************
* 功能描述：将该矩阵置0
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetZero()
{
ex.SetZero();
ey.SetZero();
ez.SetZero();
}
/**************************************************************************
* 功能描述：解决A*x = b,其中b是一个三维列向量
这比一次性求反计算更有效率
* 参数说明： b : 三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
b2Vec3 Solve33(const b2Vec3& b) const;
/**************************************************************************
* 功能描述：解决A*x = b,其中b是一个二维列向量
这比一次性求反计算更有效率
仅仅解决2X2的矩阵等式
* 参数说明： b : 三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
b2Vec2 Solve22(const b2Vec2& b) const;
/**************************************************************************
* 功能描述：获取2X2的逆矩阵,
如果出现异常则置为0矩阵
逆矩阵B = A的伴随矩阵/A的行列式
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void GetInverse22(b2Mat33* M) const;
/**************************************************************************
* 功能描述：获取3X3的对称逆矩阵
如果出现异常则置为0矩阵
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void GetSymInverse33(b2Mat33* M) const;
//矩阵的3个列向量
b2Vec3 ex, ey, ez;
};
在ccp文件中Mat33结构部分函数的实现：
//解决 A * x =b,其中b是列向量
b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
{
float32 det = b2Dot(ex, b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec3 x;
x.x = det * b2Dot(b, b2Cross(ey, ez));
x.y = det * b2Dot(ex, b2Cross(b, ez));
x.z = det * b2Dot(ex, b2Cross(ey, b));
return x;
}
//解决 A * x =b,其中b是列向量
b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
{
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
float32 det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec2 x;
x.x = det * (a22 * b.x - a12 * b.y);
x.y = det * (a11 * b.y - a21 * b.x);
return x;
}
//获取2X2的逆矩阵
//如果出现异常则置为0矩阵
void b2Mat33::GetInverse22(b2Mat33* M) const
{
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
float32 det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f;
M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f;
M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
}
//获取3X3的对称矩阵的逆矩阵，如果出现异常则置为0矩阵
void b2Mat33::GetSymInverse33(b2Mat33* M) const
{
float32 det = b2Dot(ex, b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
float32 a22 = ey.y, a23 = ez.y;
float32 a33 = ez.z;
M->ex.x = det * (a22 * a33 - a23 * a23);
M->ex.y = det * (a13 * a23 - a12 * a33);
M->ex.z = det * (a12 * a23 - a13 * a22);
M->ey.x = M->ex.y;
M->ey.y = det * (a11 * a33 - a13 * a13);
M->ey.z = det * (a13 * a12 - a11 * a23);
M->ez.x = M->ex.z;
M->ez.y = M->ey.z;
M->ez.z = det * (a11 * a22 - a12 * a12);
}
三、 旋度、转换、扫描部分
1、 旋度的实现
//旋度 http://zh.wikipedia.org/wiki/旋度
struct b2Rot
{
/**************************************************************************
* 功能描述：默认构造函数不做任何事情(仅仅为了性能)
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Rot() {}
/**************************************************************************
* 功能描述：用角的弧度初始化
* 参数说明： anlge:弧度值
* 返 回 值： (void)
**************************************************************************/
explicit b2Rot(float32 angle)
{
/// TODO_ERIN optimize
s = sinf(angle);
c = cosf(angle);
}
/**************************************************************************
* 功能描述：用角的弧度设置
* 参数说明： anlge:弧度值
* 返 回 值： (void)
**************************************************************************/
void Set(float32 angle)
{
/// TODO_ERIN optimize
s = sinf(angle);
c = cosf(angle);
}
/**************************************************************************
* 功能描述：设置成单位旋度
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetIdentity()
{
s = 0.0f;
c = 1.0f;
}
/**************************************************************************
* 功能描述：用弧度得到角度
* 参数说明： (void)
* 返 回 值： 角度值
**************************************************************************/
float32 GetAngle() const
{
return b2Atan2(s, c);
}
/**************************************************************************
* 功能描述：获取x-axis
* 参数说明： (void)
* 返 回 值： 列向量
**************************************************************************/
b2Vec2 GetXAxis() const
{
return b2Vec2(c, s);
}
/**************************************************************************
* 功能描述：获取u-axis
* 参数说明： (void)
* 返 回 值： 列向量
**************************************************************************/
b2Vec2 GetYAxis() const
{
return b2Vec2(-s, c);
}
//定义sin和cos
float32 s, c;
};
2、 变换的实现
///变换包括平移和旋度，它是用来表示位置和精确方向的框架
struct b2Transform
{
/**************************************************************************
* 功能描述：默认构造函数，什么都不做
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
b2Transform() {}
/**************************************************************************
* 功能描述：用一个位置向量和一个旋度进行初始化
* 参数说明： position ：位置向量
rotation ：旋度
* 返 回 值： (void)
**************************************************************************/
b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
/**************************************************************************
* 功能描述：设置单位变换
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void SetIdentity()
{
p.SetZero();
q.SetIdentity();
}
/**************************************************************************
* 功能描述：基于角的弧度和位置设置
* 参数说明： position：位置向量
angle ：角的弧度
* 返 回 值： (void)
**************************************************************************/
void Set(const b2Vec2& position, float32 angle)
{
p = position;
q.Set(angle);
}
//成员变量，一个向量和一个旋度
b2Vec2 p;
b2Rot q;
};
3、 扫描的实现
//描述运动的body/shape的TOI(Time of Impact)的计算。
//形状相对于主体原点定义的，这可能没有一致的原点。
//但是，为了支持动态我们必须篡改质心位置
struct b2Sweep
{
/**************************************************************************
* 功能描述：在特定时间里获得窜改变换
* 参数说明： xfb :变换的指针
beta :一个因子，在[0,1]之间，0表示alpha0
* 返 回 值： (void)
**************************************************************************/
void GetTransform(b2Transform* xfb, float32 beta) const;
/**************************************************************************
* 功能描述：推进缓慢向前移动，产生一个新的初始状态
* 参数说明：alpha:新的初始时间
* 返 回 值： (void)
**************************************************************************/
void Advance(float32 alpha);
/**************************************************************************
* 功能描述：标准化角度
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
void Normalize();
b2Vec2 localCenter; ///质心位置
b2Vec2 c0, c; ///世界的中心位置
float32 a0, a; ///世界角度
//分数，当前时间步长，范围[0,1]之间
float32 alpha0;
};
四、 其它
在b2Math.h的开始部分
/**************************************************************************
* 功能描述：用来确保一个浮点数不是一个NaN或无穷大
* 参数说明： x :数值
* 返 回 值： true ：有效
false: 无效
**************************************************************************/
inline bool b2IsValid(float32 x)
{
if (x != x)
{
// NaN.
return false;
}
float32 infinity = std::numeric_limits<float32>::infinity();
return -infinity < x && x < infinity;
}
/**************************************************************************
* 功能描述：一个近似且快速的平方根倒数算法[x^ (-1/2) ]
可以参照 http://zh.wikipedia.org/wiki/平方根倒数速算法
或 http://en.wikipedia.org/wiki/Fast_inverse_square_root
* 参数说明： x :数值
* 返 回 值： 平方根倒数
**************************************************************************/
inline float32 b2InvSqrt(float32 x)
{
union
{
float32 x;
int32 i;
} convert;
convert.x = x;
float32 xhalf = 0.5f * x;
convert.i = 0x5f3759df - (convert.i >> 1);
x = convert.x;
x = x * (1.5f - xhalf * x * x); //第一次牛顿迭代
return x;
}
//平方根宏
#define b2Sqrt(x) std::sqrt(x)
//对于任意不同时等于0的实参数x和y，
//atan2(y,x)所表达的意思是坐标原点为起点，
//指向(y,x)的射线在坐标平面上与x轴正方向之间的角的角度度
//参照 http://zh.wikipedia.org/wiki/Atan2
#define b2Atan2(y, x) std::atan2(y, x)
/**************************************************************************
* 功能描述：计算两个向量的点积,注：a = b时，表示向量长度的平方
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 点积值
**************************************************************************/
inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
{
return a.x * b.x + a.y * b.y;
}
/**************************************************************************
* 功能描述：计算两个向量的叉积，在2d中这是一个标量
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 叉积值
**************************************************************************/
inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
{
return a.x * b.y - a.y * b.x;
}
结尾部分：
/**************************************************************************
* 功能描述：计算向量和标量的叉积，在2d中这是一个标量
* 参数说明： a :二维列向量
s :标量
* 返 回 值： 叉积值
**************************************************************************/
inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
{
return b2Vec2(s * a.y, -s * a.x);
}
/**************************************************************************
* 功能描述：计算标量和向量的叉积，在2d中这是一个标量
* 参数说明： s :标量
a :二维列向量
* 返 回 值： 叉积值
**************************************************************************/
inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
{
return b2Vec2(-s * a.y, s * a.x);
}
/**************************************************************************
* 功能描述：一个矩阵乘以一个向量
* 参数说明： a :2X2矩阵
v :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
{
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
}
/**************************************************************************
* 功能描述：一个转置矩阵乘以一个向量
* 参数说明： a :2X2矩阵
v :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
{
return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
}
/**************************************************************************
* 功能描述：计算两个向量按分量逐个相加
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(a.x + b.x, a.y + b.y);
}
/**************************************************************************
* 功能描述：计算两个向量按分量逐个相减
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(a.x - b.x, a.y - b.y);
}
/**************************************************************************
* 功能描述： 计算标量和向量相乘
* 参数说明： s :标量
a :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 operator * (float32 s, const b2Vec2& a)
{
return b2Vec2(s * a.x, s * a.y);
}
/**************************************************************************
* 功能描述： 判断两个向量是否相等
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： true ：相等
flase：不相等
**************************************************************************/
inline bool operator == (const b2Vec2& a, const b2Vec2& b)
{
return a.x == b.x && a.y == b.y;
}
/**************************************************************************
* 功能描述： 计算两向量之间的距离
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 距离
**************************************************************************/
inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return c.Length();
}
/**************************************************************************
* 功能描述： 获取两向量之间距离的平方
* 参数说明： a :二维列向量
b :二维列向量
* 返 回 值： 距离的平方
**************************************************************************/
inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return b2Dot(c, c);
}
/**************************************************************************
* 功能描述： 一个标量和一个3维列向量相乘
* 参数说明： s :标量
a :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Vec3 operator * (float32 s, const b2Vec3& a)
{
return b2Vec3(s * a.x, s * a.y, s * a.z);
}
/**************************************************************************
* 功能描述： 计算两个向量按分量逐个相加
* 参数说明： a :三维列向量
b :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}
/**************************************************************************
* 功能描述： 计算两个向量按分量逐个相减
* 参数说明： a :三维列向量
b :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}
/**************************************************************************
* 功能描述： 计算两个向量的点积
* 参数说明： a :三维列向量
b :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/**************************************************************************
* 功能描述： 计算两个相邻的叉积
* 参数说明： a :三维列向量
b :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
/**************************************************************************
* 功能描述： 计算两个2X2矩阵相加
* 参数说明： a :2X2矩阵
b :2X2矩阵
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
{
return b2Mat22(A.ex + B.ex, A.ey + B.ey);
}
/**************************************************************************
* 功能描述： 计算两个2X2矩阵相乘
* 参数说明： a :2X2矩阵
b :2X2矩阵
* 返 回 值： 2X2矩阵
**************************************************************************/
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
{
return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
}
/**************************************************************************
* 功能描述： 计算一个转置矩阵和矩阵相乘
* 参数说明： a :2X2矩阵
b :2X2矩阵
* 返 回 值： 2X2矩阵
**************************************************************************/
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
{
b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
return b2Mat22(c1, c2);
}
/**************************************************************************
* 功能描述： 计算一个矩阵和向量相乘
* 参数说明： a :3X3矩阵
v :三维列向量
* 返 回 值： 三维列向量
**************************************************************************/
inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
{
return v.x * A.ex + v.y * A.ey + v.z * A.ez;
}
/**************************************************************************
* 功能描述： 计算一个矩阵和二维列向量相乘
* 参数说明： a :3X3矩阵
v :二维列向量
* 返 回 值： 二维列向量
**************************************************************************/
inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
{
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
}
/**************************************************************************
* 功能描述： 计算两个旋度的乘积
* 参数说明： q :旋度
r :旋度
* 返 回 值： 旋度
**************************************************************************/
inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
{
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
// s = qs * rc + qc * rs
// c = qc * rc - qs * rs
b2Rot qr;
qr.s = q.s * r.c + q.c * r.s;
qr.c = q.c * r.c - q.s * r.s;
return qr;
}
/**************************************************************************
* 功能描述： 计算一个转置旋度和一个旋度的乘积
* 参数说明： q :旋度
r :旋度
* 返 回 值： 旋度
**************************************************************************/
inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
b2Rot qr;
qr.s = q.c * r.s - q.s * r.c;
qr.c = q.c * r.c + q.s * r.s;
return qr;
}
/**************************************************************************
* 功能描述： 旋转一个向量
* 参数说明： q :旋度
v :向量
* 返 回 值： 向量
**************************************************************************/
inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
{
// [qc -qs] * [vx] = [qc*vx-qs*vy qs*vx+qc*vy]
// [qs qc] [vy]
return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
}
/**************************************************************************
* 功能描述： 反旋转一个向量
* 参数说明： q :旋度
v :向量
* 返 回 值： 向量
**************************************************************************/
inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
{
// [ qc qs] * [vx] = [qc*vx+qs*vy -qs*vx+qc*vy]
// [-qs qc] [vy]
return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
}
/**************************************************************************
* 功能描述： 变换一个向量
* 参数说明： T :变换
v :向量
* 返 回 值： 向量
**************************************************************************/
inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
{
// [qc -qs] * [vx] = [qc*vx-qs*vy qs*vx+qc*vy]
// [qs qc] [vy]
float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
return b2Vec2(x, y);
}
/**************************************************************************
* 功能描述： 反变换一个向量
* 参数说明： T :变换
v :向量
* 返 回 值： 向量
**************************************************************************/
inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
{
//( [vx] - [tpx] ) *[ qc qs] = [vx - tpx] *[ qc qs]
//( [vy] [tpy] ) [-qs qc] [vy - tpy] [-qs qc]
float32 px = v.x - T.p.x;
float32 py = v.y - T.p.y;
float32 x = (T.q.c * px + T.q.s * py);
float32 y = (-T.q.s * px + T.q.c * py);
return b2Vec2(x, y);
}
/**************************************************************************
* 功能描述： 变换一个变换
v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
= (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
* 参数说明： a :变换
b :变换
* 返 回 值： 变换
**************************************************************************/
inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
{
b2Transform C;
C.q = b2Mul(A.q, B.q);
C.p = b2Mul(A.q, B.p) + A.p;
return C;
}
/**************************************************************************
* 功能描述：反变换一个变换
v2 = A.q' * (B.q * v1 + B.p - A.p)
= A.q' * B.q * v1 + A.q' * (B.p - A.p)
* 参数说明： a：变换
b:变换
* 返 回 值： 变换
***************************************************************************/
inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
{
b2Transform C;
C.q = b2MulT(A.q, B.q);
C.p = b2MulT(A.q, B.p - A.p);
return C;
}
/**************************************************************************
* 功能描述：取绝对值函数
* 参数说明： a：标量
* 返 回 值： 标量
***************************************************************************/
template <typename T>
inline T b2Abs(T a)
{
return a > T(0) ? a : -a;
}
/**************************************************************************
* 功能描述：取向量的绝对值
* 参数说明： A：二维列向量
* 返 回 值： 二维列向量
***************************************************************************/
inline b2Vec2 b2Abs(const b2Vec2& a)
{
return b2Vec2(b2Abs(a.x), b2Abs(a.y));
}
/**************************************************************************
* 功能描述：取2X2矩阵的绝对值
* 参数说明： A：2X2矩阵
* 返 回 值： 2X2矩阵
***************************************************************************/
inline b2Mat22 b2Abs(const b2Mat22& A)
{
return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
}
/**************************************************************************
* 功能描述：获得最小值函数
* 参数说明： a : 标量
b : 标量
* 返 回 值： 标量
***************************************************************************/
template <typename T>
inline T b2Min(T a, T b)
{
return a < b ? a : b;
}
/**************************************************************************
* 功能描述：获得最小向量值
* 参数说明： a : 二维向量
b : 二维向量
* 返 回 值： 二维向量
***************************************************************************/
inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
}
/**************************************************************************
* 功能描述：获得最大值函数
* 参数说明： a : 标量
b : 标量
* 返 回 值： 标量
***************************************************************************/
template <typename T>
inline T b2Max(T a, T b)
{
return a > b ? a : b;
}
/**************************************************************************
* 功能描述：获得最大向量值
* 参数说明： a : 二维向量
b : 二维向量
* 返 回 值： 二维向量
***************************************************************************/
inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
}
/**************************************************************************
* 功能描述：获得low到high之间的值
* 参数说明： a ，low,high:都是标量
* 返 回 值： 标量
***************************************************************************/
template <typename T>
inline T b2Clamp(T a, T low, T high)
{
return b2Max(low, b2Min(a, high));
}
/**************************************************************************
* 功能描述：获得两向量之间的向量
* 参数说明： a ，low,high:都是二维向量
* 返 回 值： 二维向量
***************************************************************************/
inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
{
return b2Max(low, b2Min(a, high));
}
/**************************************************************************
* 功能描述：交换两个标量
* 参数说明： (void)
* 返 回 值： (void)
**************************************************************************/
template<typename T>
inline void b2Swap(T& a, T& b)
{
T tmp = a;
a = b;
b = tmp;
}
/**************************************************************************
* 功能描述：下一个最大2的幂
给你一个二进制整数x。
通过swar算法计算出下一个最大2的幂
递归的折叠高位bit到低位中去。
* 参数说明： x:需要判断的值
* 返 回 值： (void)
**************************************************************************/
inline uint32 b2NextPowerOfTwo(uint32 x)
{
x |= (x >> 1);
x |= (x >> 2);
x |= (x >> 4);
x |= (x >> 8);
x |= (x >> 16);
return x + 1;
}
/**************************************************************************
* 功能描述：判断是否是2的幂
* 参数说明： x:需要判断的值
* 返 回 值： (void)
**************************************************************************/
inline bool b2IsPowerOfTwo(uint32 x)
{
bool result = x > 0 && (x & (x - 1)) == 0;
return result;
}
/**************************************************************************
* 功能描述：在特定时间里获得窜改变换
* 参数说明： xfb :变换的指针
beta :一个因子，在[0,1]之间，0表示alpha0
* 返 回 值： (void)
**************************************************************************/
inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
{
xf->p = (1.0f - beta) * c0 + beta * c;
float32 angle = (1.0f - beta) * a0 + beta * a;
xf->q.Set(angle);
// Shift to origin
xf->p -= b2Mul(xf->q, localCenter);
}
/**************************************************************************
* 功能描述：推进缓慢向前移动，产生一个新的初始状态
* 参数说明：alpha:新的初始时间
* 返 回 值： (void)
**************************************************************************/
inline void b2Sweep::Advance(float32 alpha)
{
b2Assert(alpha0 < 1.0f);
float32 beta = (alpha - alpha0) / (1.0f - alpha0);
c0 = (1.0f - beta) * c0 + beta * c;
a0 = (1.0f - beta) * a0 + beta * a;
alpha0 = alpha;
}
/**************************************************************************
* 功能描述：标准化角（以弧度为单位）在-pi和pi之间
* 参数说明：alpha:新的初始时间
* 返 回 值： (void)
**************************************************************************/
inline void b2Sweep::Normalize()
{
float32 twoPi = 2.0f * b2_pi;
float32 d = twoPi * floorf(a0 / twoPi);
a0 -= d;
a -= d;
}
还有就是b2Vec2_zero的声明和定义：
声明：
//有用的常量
extern const b2Vec2 b2Vec2_zero;
定义：
//声明一个二维零向量
const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
以上就是box2d中所使用的数学库和相关的API接口，上面的注释已详细说明其作用，这里就不在阐述了。如遇到相关数学方面的知识，个人建议维基百科，那里比较全，而且很详细。另外，如有什么不妥、错误之处望高手指出。
