8．第一次代码重构
前面的几篇文章我们已经建立起了一个基本的物理引擎，下面就要开始处理碰撞了，对于碰撞来说，难的不是物理而是数学，为了让引擎的结构更好地适应碰撞的处理，我们需要进行一些调整，让它更健壮。
添加数学和变换辅助类
在引擎中添加Common目录，在这个目录中添加2D向量类Vector2.cs，3D向量类Vector3.cs，矩阵类Matrix.cs，数学函数辅助类MathHelper.cs，这四个函数在XNA框架中都是自带的，但如果引擎中工作在Silverlight环境中，就无法使用XNA框架中的这四个类，因此需要自己创建，其中Vector2类我们已经在前面写过了，只需要自己写另外三个类。幸运的是有现成代码可以使用，这就是 MonoXna(www.monoxna.org)，它可以看成XNA的开源代码，并支持多平台，但现在好像很久没有更新了，不过我们可以参考（也可以说是照抄）它的源代码，以上四个类的代码就来自于它的源代码(http://code.google.com/p/monoxna/source/browse/#svn%2Ftrunk%2Fsrc)。
源代码比较长，就不贴出了，只需要说明一点，以Vector2.cs为例：
#if(!XNA)
using System;
using System.Runtime.InteropServices;
using System.Text;
namespace Microsoft.Xna.Framework
{
[StructLayout(LayoutKind.Sequential)]
public struct Vector2 : IEquatable<Vector2>
{
[…]
}
}
#endif
从代码中我们可以看到使用了条件编译#if(!XNA)，因此当编译目标不是XNA时（这里是Silverlight），就不会编译这个类，而且这四个类的命名空间都是Microsoft.Xna.Framework。这样做带来的好处是Stun2Dphysics的Silverlight和XNA的版本的代码文件是完全一样了，无需进行微调避免不必要的错误，在Silverlight平台上引擎会编译这个Vector2类，而XNA平台就不会编译这个类而使用XNA框架自带的Vector2类。
在Common目录下还有一个Math.cs文件，它由四部分组成
1．Mat22结构体
它是一个自定义的2x2矩阵，代码如下：
////// 2*2的列主序矩阵。 /// public struct Mat22 { ////// 矩阵的第一列数据。 /// public Vector2 Col1; ////// 矩阵的第二列数据。 /// public Vector2 Col2; ////// 使用两列构建矩阵。 /// /// 第一列对应的二维矢量。 /// 第二列对应的二维矢量。 public Mat22(Vector2 c1, Vector2 c2) { Col1 = c1; Col2 = c2; } ////// 使用4个标量构建矩阵。 /// /// a11。 /// a12。 /// a21。 /// a22。 public Mat22(float a11, float a12, float a21, float a22) { Col1 = new Vector2(a11, a21); Col2 = new Vector2(a12, a22); } ////// 使用角度构建矩阵。此时这个矩阵成为一个正交旋转矩阵。 /// /// 角度。 public Mat22(float angle) { float c = (float)Math.Cos(angle), s = (float)Math.Sin(angle); Col1 = new Vector2(c, s); Col2 = new Vector2(-s, c); } ////// 从矩阵中提取旋转角度。 /// public float Angle { get { return (float)Math.Atan2(Col1.Y, Col1.X); } } ////// 计算逆矩阵 /// public Mat22 Inverse { get { float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y; // 计算矩阵行列式 float det = a * d - b * c; if (det != 0.0f) { det = 1.0f / det; } Mat22 result = new Mat22(); result.Col1.X = det * d; result.Col1.Y = -det * c; result.Col2.X = -det * b; result.Col2.Y = det * a; return result; } } ////// 使用列设置矩阵。 /// /// 第一列对应的二维矢量。 /// 第二列对应的二维矢量。 public void Set(Vector2 c1, Vector2 c2) { Col1 = c1; Col2 = c2; } ////// 使用旋转角度值设置矩阵。 /// 这个矩阵就成为一个正交旋转矩阵。 /// /// 旋转角度。 public void Set(float angle) { float c = (float)Math.Cos(angle); float s = (float)Math.Sin(angle); Col1.X = c; Col2.X = -s; Col1.Y = s; Col2.Y = c; } ////// 设置为单位矩阵。 /// public void SetIdentity() { Col1.X = 1.0f; Col2.X = 0.0f; Col1.Y = 0.0f; Col2.Y = 1.0f; } ////// 矩阵清零。 /// public void SetZero() { Col1.X = 0.0f; Col2.X = 0.0f; Col1.Y = 0.0f; Col2.Y = 0.0f; } ////// 矩阵相加 /// /// 第一个矩阵 /// 第二个矩阵 /// 相加结果矩阵 public static void Add(ref Mat22 A, ref Mat22 B, out Mat22 R) { R.Col1 = A.Col1 + B.Col1; R.Col2 = A.Col2 + B.Col2; } }
XNA框架自带的Matrix是4x4结构，其中左上角的3*3矩阵包含了旋转和缩放的信息，同理，要处理2D的情况，我们可以使用一个2x2矩阵表示2D物体的旋转（无需处理缩放的情况），例如，一个对象顺时针旋转θ，可以用下图表示：
图1 旋转矩阵的定义
即对象的本地坐标系（红色）相对于世界坐标系（黑色）顺时针旋转了θ，此时红色x轴在世界坐标系中的坐标（不考虑缩放，即认为长度为1）为（cosθ，sinθ），y轴的坐标为（－sinθ，cosθ），因为Mat22是列主序的，所以将这两个坐标作为两列构成一个2x2矩阵，而正是这个矩阵包含了物体的旋转信息，即角位移信息。
其实只用一个浮点数θ就可以表示2D空间中的旋转，为什么需要使用4个浮点数构成矩阵？这是因为矩阵最主要的优点是可以立即进行向量的旋转，而且图形API就是使用矩阵来描述旋转。同理，在3D空间中只需使用3个浮点数表示三个欧拉角，但我们使用的却是9个浮点数构成的3*3矩阵。
2．Transform结构
在XNA中，我们使用一个4*4矩阵描述3D空间中物体的位置和朝向，同理，在2D空间中我们可以使用3*3矩阵表示物体的位置和朝向，但为了减少数据冗余，我们使用一个Vector2表示位置，一个Mat22表示朝向，组合成Transform结构。这个结构就可以描述Body的位置和旋转了。代码如下：
/// <summary>
/// 包含平移和旋转的变换，用于表示刚体的位置和朝向。
/// </summary>
public struct Transform
{
/// <summary>
/// 位置
/// </summary>
public Vector2 Position;
/// <summary>
/// 表示旋转的2*2矩阵
/// </summary>
public Mat22 RMatrix;
/// <summary>
/// 使用一个位置矢量和一个旋转矩阵进行初始化。
/// </summary>
// <param name="position">位置矢量。</param>
/// <param name="r">旋转矩阵。</param>
public Transform(ref Vector2 position, ref Mat22 rotation)
{
Position = position;
RMatrix = rotation;
}
/// <summary>
/// 计算旋转矩阵表示的旋转角度。
/// </summary>
public float Angle
{
get { return (float)Math.Atan2(RMatrix.Col1.Y, RMatrix.Col1.X);}
}
/// <summary>
/// 设置为单位变换。即将位置设置为零矢量，旋转矩阵设置为单位矩阵
/// </summary>
public void SetIdentity()
{
Position = Vector2.Zero;
RMatrix.SetIdentity();
}
/// <summary>
/// 基于位置和旋转角度设置变换。
/// </summary>
/// <param name="position">位置矢量。</param>
/// <param name="angle">旋转角度。</param>
public void Set(Vector2 position, float angle)
{
Position = position;
RMatrix.Set(angle);
}
}
3．Sweep结构
我们还创建了一个结构保存Body的质心位置和旋转信息，代码如下：
////// 保存Body质心位置和旋转的结构体。 /// public struct Sweep { ////// 世界空间中的旋转量 /// public float A; ////// 世界空间中的旋转量备份 /// public float A0; ////// 质心的世界坐标 /// public Vector2 C; ////// 质心的世界坐标备份 /// public Vector2 C0; ////// 质心的本地坐标 /// public Vector2 LocalCenter; }
为什么这个结构叫做Sweep？这会涉及到以后（可能是很久以后）将要讨论的连续碰撞检测（Continuous Collision Detection，简称CCD），其中有个扫掠体（swept volume）的概念。
4．静态MathUtils类
这个类包含一些前面的MathHelper类没有包含的辅助方法，代码如下：
public static class MathUtils
{
/// <summary>
/// 计算二维矢量的绝对值
/// </summary>
/// <param name="v">二维矢量</param>
public static Vector2 Abs(Vector2 v)
{
return new Vector2(Math.Abs(v.X), Math.Abs(v.Y));
}
/// <summary>
/// 交换两个对象的值。
/// </summary>
/// <typeparam name="T">对象类型。</typeparam>
/// <param name="a">第一个对象。</param>
/// <param name="b">第二个对象。</param>
public static void Swap<T>(ref T a, ref T b)
{
T tmp = a;
a = b;
b = tmp;
}
/// <summary>
/// 判断一个浮点数是否是有效值，即是不是NaN或无穷大。
/// </summary>
public static bool IsValid(float x)
{
if (float.IsNaN(x))
{
// NaN.
return false;
}
return !float.IsInfinity(x);
}
/// <summary>
/// 判断一个二维矢量是否是有效值。
/// </summary>
/// <param name="x"></param>
/// <returns></returns>
public static bool IsValid(this Vector2 x)
{
return IsValid(x.X) && IsValid(x.Y);
}
/// <summary>
/// 拟叉乘，两个二维矢量拟叉乘的结果是一个浮点数
/// </summary>
/// <param name="a">第一个二维矢量</param>
/// <param name="b">第二个二维矢量</param>
public static float Cross(Vector2 a, Vector2 b)
{
return a.X * b.Y - a.Y * b.X;
}
/// <summary>
/// 拟叉乘，两个二维矢量拟叉乘的结果是一个浮点数
/// </summary>
/// <param name="a">第一个二维矢量</param>
/// <param name="b">第二个二维矢量</param>
/// <param name="c">结果浮点数。</param>
public static void Cross(ref Vector2 a, ref Vector2 b, out float c)
{
c = a.X * b.Y - a.Y * b.X;
}
/// <summary>
/// 使用一个2*2矩阵对二维矢量进行变换，只变换旋转。
/// </summary>
/// <param name="A">2*2矢量。</param>
/// <param name="v">二维矢量。</param>
/// <returns>经过变换的二维矢量。</returns>
public static Vector2 Multiply(ref Mat22 A, Vector2 v)
{
return Multiply(ref A, ref v);
}
/// <summary>
/// 使用一个2*2矩阵对二维矢量进行变换，只变换旋转。
/// </summary>
/// <param name="A">2*2矢量。</param>
/// <param name="v">二维矢量。</param>
/// <returns>经过变换的二维矢量。</returns>
public static Vector2 Multiply(ref Mat22 A, ref Vector2 v)
{
return new Vector2(A.Col1.X * v.X + A.Col2.X * v.Y, A.Col1.Y * v.X + A.Col2.Y * v.Y);
}
/// <summary>
/// 对一个二维矢量进行变换，同时变换平移和旋转。
/// </summary> /// <param name="T">变换。</param>
/// <param name="v">二维矢量。</param>
/// <returns>经过变换的二维矢量。</returns>
public static Vector2 Multiply(ref Transform T, Vector2 v)
{
return Multiply(ref T, ref v);
}
/// <summary>
/// 对一个二维矢量进行变换，同时变换平移和旋转。
/// </summary>
/// <param name="T">变换。</param>
/// <param name="v">二维矢量。</param>
/// <returns>经过变换的二维矢量。</returns>
public static Vector2 Multiply(ref Transform T, ref Vector2 v)
{
return new Vector2(T.Position.X + T.RMatrix.Col1.X * v.X + T.RMatrix.Col2.X * v.Y, T.Position.Y + T.RMatrix.Col1.Y * v.X + T.RMatrix.Col2.Y * v.Y);
}
/// <summary>
/// 对一个整数进行截取。
/// </summary>
/// <param name="a">要截取的整数。</param>
/// <param name="low">下限。</param>
/// <param name="high">上限。</param>
public static int Clamp(int a, int low, int high)
{
return Math.Max(low, Math.Min(a, high));
}
/// <summary>
/// 对一个浮点数进行截取。
/// </summary>
/// <param name="a">要截取的浮点数。</param>
/// <param name="low">下限。</param>
/// <param name="high">上限。</param>
public static float Clamp(float a, float low, float high) { return Math.Max(low, Math.Min(a, high)); }
/// <summary>
/// 对一个二维矢量进行截取。
/// </summary>
/// <param name="a">要截取的二维矢量。</param>
/// <param name="low">下限。</param>
/// <param name="high">上限。</param>
public static Vector2 Clamp(Vector2 a, Vector2 low, Vector2 high)
{
return Vector2.Max(low, Vector2.Min(a, high));
}
}
Body类
先贴出Body类的代码，然后就细节部分进行解释：
using System;
using System.Diagnostics;
using Stun2DPhysics4SL.Common;
using Microsoft.Xna.Framework;
namespace Stun2DPhysics4SL.Dynamics
{
/// <summary>
/// Body类型。
/// </summary>
public enum BodyType
{
/// <summary>
/// 速度为零即静止不动，可以手动移动，注意：静止Body也有质量。
/// </summary>
Static,
/// <summary>
/// 质量为正值, 施加力可以产生速度。
/// </summary>
Dynamic,
}
[Flags]
public enum BodyFlags
{
None = 0,
Enabled = (1 << 0),
IgnoreGravity = (1 << 1),
}
public class Body : IDisposable
{
/// <summary>
/// Body编号
/// </summary>
public int BodyId;
private static int _bodyIdCounter; // Body编号，它是一个静态变量
internal BodyFlags Flags; // 包含Body属性信息的标志
private BodyType _bodyType; // Body类型，目前有Static（静态）和Dynamic（动态）两种
private float _mass; // 质量
internal float InvMass; // 质量倒数
internal Vector2 LinearVelocityInternal; // 引擎内部调用的线速度
internal Vector2 Force; // 施加在Body上的力
private float _inertia; // 以质心为转轴的转动惯量
internal float InvInertia; // 转动惯量的倒数
internal float AngularVelocityInternal; // 引擎内部调用的角速度
internal float Torque; // 施加在Body上的力矩
internal Transform Xf; // Body原点的变换
internal Sweep Sweep; // 保存Body质心位置和旋转信息的结构体
internal World World;
/// <summary>
/// 创建一个Body对象
/// </summary>
/// <param name="world">对World的引用。</param>
public Body(World world)
{
BodyId = _bodyIdCounter++;
BodyType = BodyType.Dynamic ;
Enabled = true;
Xf.RMatrix.Set(0);
World = world;
world.AddBody(this);
}
#region 属性
/// <summary>
/// 获取或设置Body类型。
/// </summary>
public BodyType BodyType
{
get { return _bodyType; }
set
{
if (_bodyType == value)
{
return;
}
_bodyType = value;
// 如果将Body类型设置为静态的，则需要将线速度和角速度都设置为0
if (_bodyType == BodyType.Static)
{
LinearVelocityInternal = Vector2.Zero;
AngularVelocityInternal = 0.0f;
}
Force = Vector2.Zero;
Torque = 0.0f;
}
}
/// <summary>
/// 获取或设置质心的线速度。
/// </summary>
public Vector2 LinearVelocity
{
set
{
// 如果是静态对象，则直接返回。
if (_bodyType == BodyType.Static)
return;
LinearVelocityInternal = value;
}
get { return LinearVelocityInternal; }
}
/// <summary>
/// 获取或设置角速度，单位为弧度每秒(rad/s)。
/// </summary>
public float AngularVelocity
{
set
{
// 如果是静态对象，则直接返回。
if (_bodyType == BodyType.Static)
return;
AngularVelocityInternal = value;
}
get { return AngularVelocityInternal; }
}
/// <summary>
/// 获取或设置线性阻尼系数。
/// </summary>
public float LinearDamping{ get;set; }
/// <summary>
/// 获取或设置角度阻尼系数。
/// </summary>
public float AngularDamping{ get;set; }
/// <summary>
/// 设置Body的激活状态。
/// </summary>
public bool Enabled
{
set
{
if (value == Enabled)
{
return;
}
if (value)
{
Flags |= BodyFlags.Enabled;
}
else
{
Flags &= ~BodyFlags.Enabled;
}
}
get { return (Flags & BodyFlags.Enabled) == BodyFlags.Enabled; }
}
/// <summary>
/// 获取或设置Body在世界坐标系中的初始位置。
/// </summary>
public Vector2 Position
{
get { return Xf.Position; }
set
{
SetTransform(ref value, Rotation);
}
}
/// <summary>
/// 获取或设置旋转量，单位为弧度。
/// </summary>
public float Rotation
{
get { return Sweep.A; }
set
{
SetTransform(ref Xf.Position, value);
}
}
/// <summary>
/// 获取或设置Body是否处是静态的。
/// </summary>
public bool IsStatic
{
get { return _bodyType == BodyType.Static; }
set
{
if (value)
BodyType = BodyType.Static;
else
BodyType = BodyType.Dynamic;
}
}
/// <summary>
/// 获取或设置Body是否忽略本身的重力。
/// </summary>
public bool IgnoreGravity
{
get { return (Flags & BodyFlags.IgnoreGravity) == BodyFlags.IgnoreGravity; }
set
{
if (value)
Flags |= BodyFlags.IgnoreGravity;
else
Flags &= ~BodyFlags.IgnoreGravity;
}
}
/// <summary>
/// 获取质心在世界坐标系中的坐标。
/// </summary>
public Vector2 WorldCenter
{
get { return Sweep.C; }
}
/// <summary>
/// 获取或设置质心的本地坐标。
/// </summary>
public Vector2 LocalCenter
{
get { return Sweep.LocalCenter; }
set
{
if (_bodyType != BodyType.Dynamic)
return;
// 更新存储在Sweep中的质心本地坐标和世界坐标。
Vector2 oldCenter = Sweep.C;
Sweep.LocalCenter = value;
Sweep.C0 = Sweep.C = MathUtils.Multiply(ref Xf, ref Sweep.LocalCenter);
// 更新质心的速度。
Vector2 a = Sweep.C - oldCenter;
LinearVelocityInternal += new Vector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X);
}
}
/// <summary>
/// 获取或设置质量，单位为千克(kg)。
/// </summary>
public float Mass
{
get { return _mass; }
set
{
if (_bodyType != BodyType.Dynamic)
return;
_mass = value;
if (_mass <= 0.0f)
_mass = 1.0f;
InvMass = 1.0f / _mass;
}
}
/// <summary>
/// 获取或设置body相对于本地坐标原点的转动惯量，单位为kg-m^2。
/// </summary>
public float Inertia
{
get { return _inertia + Mass * Vector2.Dot(Sweep.LocalCenter, Sweep.LocalCenter); }
set
{
if (_bodyType != BodyType.Dynamic)
return;
if (value > 0.0f)
{
_inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter);
InvInertia = 1.0f / _inertia;
}
}
}
#endregion
#region 施加力、力矩、冲量、角冲量的方法
/// <summary>
/// 在质心上施加一个力。
/// </summary>
/// <param name="force">力。</param>
public void ApplyForce(ref Vector2 force)
{
ApplyForce(ref force, ref Xf.Position);
}
/// <summary>
/// 在质心上施加一个力。
/// </summary>
/// <param name="force">力。</param>
public void ApplyForce(Vector2 force)
{
ApplyForce(ref force, ref Xf.Position);
}
/// <summary>
/// 在世界坐标系中某点施加一个力。
/// 如果这个力没有通过质心，则还会产生一个力矩并影响到角速度。
/// </summary>
/// <param name="force">世界坐标系中的力，单位为牛顿(N)。</param>
/// <param name="point">世界坐标系中的点位置。</param>
public void ApplyForce(Vector2 force, Vector2 point)
{
ApplyForce(ref force, ref point);
}
/// <summary>
/// 在世界坐标系中某点施加一个力。
/// 如果这个力没有通过质心，则还会产生一个力矩并影响到角速度。
/// </summary>
/// <param name="force">世界坐标系中的力，单位为牛顿(N)。</param>
/// <param name="point">世界坐标系中的点位置。</param>
public void ApplyForce(ref Vector2 force, ref Vector2 point)
{
if (_bodyType == BodyType.Dynamic)
{
Force += force;
Torque += (point.X - Sweep.C.X) * force.Y - (point.Y - Sweep.C.Y) * force.X;
}
}
/// <summary>
/// 施加一个力矩。这会影响到角速度但不会影响质心的线速度。
/// </summary>
/// <param name="torque">关于z轴（指向屏幕之外）的力矩，单位为N·m。</param>
public void ApplyTorque(float torque)
{
if (_bodyType == BodyType.Dynamic)
{
Torque += torque;
}
}
/// <summary>
/// 在世界坐标系中施加一个冲量，可以立即改变速度。
/// </summary>
/// <param name="impulse">世界坐标系中的冲量矢量，单位为N·seconds或kg·m/s。</param>
public void ApplyLinearImpulse(Vector2 impulse)
{
ApplyLinearImpulse(ref impulse);
}
/// <summary>
/// 在世界坐标系中施加一个冲量，可以立即改变速度。
/// </summary>
/// <param name="impulse">世界坐标系中的冲量矢量，单位为N·seconds或kg·m/s。</param>
public void ApplyLinearImpulse(ref Vector2 impulse)
{
if (_bodyType != BodyType.Dynamic)
{
return;
}
LinearVelocityInternal += InvMass * impulse;
}
/// <summary>
/// 在世界坐标系中某点施加一个冲量，可以立即改变速度。
/// 如果这个冲量不通过质心，则还会改变角速度。
/// </summary>
/// <param name="impulse">世界坐标系中的冲量矢量，单位为N·seconds或kg·m/s。</param>
/// <param name="point">世界坐标系中的点位置。</param>
public void ApplyLinearImpulse(Vector2 impulse, Vector2 point)
{
ApplyLinearImpulse(ref impulse, ref point);
}
/// <summary>
/// 在世界坐标系中某点施加一个冲量，可以立即改变速度。
/// 如果这个冲量不通过质心，则还会改变角速度。
/// </summary>
/// <param name="impulse">世界坐标系中的冲量矢量，单位为N·seconds或kg·m/s。</param>
/// <param name="point">世界坐标系中的点位置。</param>
public void ApplyLinearImpulse(ref Vector2 impulse, ref Vector2 point)
{
if (_bodyType != BodyType.Dynamic)
return;
LinearVelocityInternal += InvMass * impulse;
AngularVelocityInternal += InvInertia * ((point.X - Sweep.C.X) * impulse.Y - (point.Y - Sweep.C.Y) * impulse.X);
}
/// <summary>
/// 施加一个角冲量。
/// </summary>
/// <param name="impulse">角冲量，单位为kg*m*m/s。</param>
public void ApplyAngularImpulse(float impulse)
{
// 如果Body是静态的，则施加角冲量是无用的，直接退出
if (_bodyType != BodyType.Dynamic)
{
return;
}
AngularVelocityInternal += InvInertia * impulse;
}
#endregion
#region 世界坐标和本地坐标的变换
/// <summary>
/// 设置Body原点的位置和旋转。
/// </summary>
/// <param name="position">Body原点在世界坐标系中位置。</param>
/// <param name="rotation">世界空间中的旋转量，以弧度为单位。</param>
public void SetTransform(ref Vector2 position, float rotation)
{
Xf.RMatrix.Set(rotation);
Xf.Position = position;
// 更新存储在Sweep中的质心世界坐标和旋转量
Sweep.C0 =
Sweep.C =
new Vector2(Xf.Position.X + Xf.RMatrix.Col1.X * Sweep.LocalCenter.X + Xf.RMatrix.Col2.X * Sweep.LocalCenter.Y,
Xf.Position.Y + Xf.RMatrix.Col1.Y * Sweep.LocalCenter.X + Xf.RMatrix.Col2.Y * Sweep.LocalCenter.Y);
Sweep.A0 = Sweep.A = rotation;
}
/// <summary>
/// 设置Body原点的位置和旋转。
/// </summary>
/// <param name="position">Body原点在世界坐标系中位置。</param>
/// <param name="rotation">世界空间中的旋转量，以弧度为单位。</param>
public void SetTransform(Vector2 position, float rotation)
{
SetTransform(ref position, rotation);
}
/// <summary>
/// 获取Body的变换数据。
/// </summary>
/// <param name="transform">输出的Body的变换信息。</param>
public void GetTransform(out Transform transform)
{
transform = Xf;
}
/// <summary>
/// 给定本地空间中的点，返回世界空间中的点。
/// </summary>
/// <param name="localPoint">本地空间中的点。</param>
/// <returns>世界空间中的点。</returns>
public Vector2 GetWorldPoint(ref Vector2 localPoint)
{
return new Vector2(Xf.Position.X + Xf.RMatrix.Col1.X * localPoint.X + Xf.RMatrix.Col2.X * localPoint.Y,
Xf.Position.Y + Xf.RMatrix.Col1.Y * localPoint.X + Xf.RMatrix.Col2.Y * localPoint.Y);
}
/// <summary>
/// 给定本地空间中的点，返回世界空间中的点。
/// </summary>
/// <param name="localPoint">本地空间中的点。</param>
/// <returns>世界空间中的点。</returns>
public Vector2 GetWorldPoint(Vector2 localPoint)
{
return GetWorldPoint(ref localPoint);
}
/// <summary>
/// 给定世界空间中的点，获取本地空间中的点。
/// </summary>
/// <param name="worldPoint">世界空间中的点。</param>
/// <returns>本地空间中的点。</returns>
public Vector2 GetLocalPoint(ref Vector2 worldPoint)
{
return
new Vector2((worldPoint.X - Xf.Position.X) * Xf.RMatrix.Col1.X + (worldPoint.Y - Xf.Position.Y) * Xf.RMatrix.Col1.Y,
(worldPoint.X - Xf.Position.X) * Xf.RMatrix.Col2.X + (worldPoint.Y - Xf.Position.Y) * Xf.RMatrix.Col2.Y);
}
/// <summary>
/// 给定世界空间中的点，获取本地空间中的点。
/// </summary>
/// <param name="worldPoint">世界空间中的点。</param>
/// <returns>本地空间中的点。</returns>
public Vector2 GetLocalPoint(Vector2 worldPoint)
{
return GetLocalPoint(ref worldPoint);
}
/// <summary>
/// 给定本地空间中的矢量，返回世界空间中的矢量。
/// 矢量只需关心旋转而不考虑位置。
/// </summary>
/// <param name="localVector">本地空间中的矢量。</param>
/// <returns>世界空间中的矢量。</returns>
public Vector2 GetWorldVector(ref Vector2 localVector)
{
return new Vector2(Xf.RMatrix.Col1.X * localVector.X + Xf.RMatrix.Col2.X * localVector.Y,
Xf.RMatrix.Col1.Y * localVector.X + Xf.RMatrix.Col2.Y * localVector.Y);
}
/// <summary>
/// 给定本地空间中的矢量，返回世界空间中的矢量。
/// 矢量只需关心旋转而不考虑位置。
/// </summary>
/// <param name="localVector">本地空间中的矢量。</param>
/// <returns>世界空间中的矢量。</returns>
public Vector2 GetWorldVector(Vector2 localVector)
{
return GetWorldVector(ref localVector);
}
/// <summary>
/// 给定世界空间中的矢量，获取本地空间中的矢量。
/// 这个矢量只考虑旋转不考虑位置。
/// </summary>
/// <param name="worldPoint">世界空间中的矢量。</param>
/// <returns>本地空间中的矢量。</returns>
public Vector2 GetLocalVector(ref Vector2 worldVector)
{
return new Vector2(worldVector.X * Xf.RMatrix.Col1.X + worldVector.Y * Xf.RMatrix.Col1.Y,
worldVector.X * Xf.RMatrix.Col2.X + worldVector.Y * Xf.RMatrix.Col2.Y);
}
/// <summary>
/// 给定世界空间中的矢量，获取本地空间的矢量。
/// 这个矢量只考虑旋转不考虑位置。
/// </summary>
/// <param name="worldPoint">世界空间中的矢量。</param>
/// <returns>本地空间中的矢量。</returns>
public Vector2 GetLocalVector(Vector2 worldVector)
{
return GetLocalVector(ref worldVector);
}
/// <summary>
/// 获取世界空间中的线速度矢量。
/// </summary>
/// <param name="worldPoint">世界空间中的点位置。</param>
/// <returns>点在世界空间中的线速度矢量。</returns>
public Vector2 GetLinearVelocityFromWorldPoint(Vector2 worldPoint)
{
return GetLinearVelocityFromWorldPoint(ref worldPoint);
}
/// <summary>
/// 获取世界空间中的线速度矢量。
/// </summary>
/// <param name="worldPoint">世界空间中的点位置。</param>
/// <returns>点在世界空间中的线速度矢量。</returns>
public Vector2 GetLinearVelocityFromWorldPoint(ref Vector2 worldPoint)
{
return LinearVelocityInternal +
new Vector2(-AngularVelocityInternal * (worldPoint.Y - Sweep.C.Y),
AngularVelocityInternal * (worldPoint.X - Sweep.C.X));
}
/// <summary>
/// 获取世界空间中的线速度矢量。
/// </summary>
/// <param name="localPoint">本地空间中的点位置。</param>
/// <returns>点在世界空间中的线速度矢量。</returns>
public Vector2 GetLinearVelocityFromLocalPoint(Vector2 localPoint)
{
return GetLinearVelocityFromLocalPoint(ref localPoint);
}
/// <summary>
/// 获取世界空间中的线速度矢量。
/// </summary>
/// <param name="localPoint">本地空间中的点位置。</param>
/// <returns>点在世界空间中的线速度矢量。</returns>
public Vector2 GetLinearVelocityFromLocalPoint(ref Vector2 localPoint)
{
return GetLinearVelocityFromWorldPoint(GetWorldPoint(ref localPoint));
}
/// <summary>
/// Sweep变化后同步改变Transform中的值
/// </summary>
internal void SynchronizeTransform()
{
// 改变Transform中的旋转量
Xf.RMatrix.Set(Sweep.A);
// 改变Transform中的原点位置
float vx = Xf.RMatrix.Col1.X * Sweep.LocalCenter.X + Xf.RMatrix.Col2.X * Sweep.LocalCenter.Y;
float vy = Xf.RMatrix.Col1.Y * Sweep.LocalCenter.X + Xf.RMatrix.Col2.Y * Sweep.LocalCenter.Y;
Xf.Position.X = Sweep.C.X - vx;
Xf.Position.Y = Sweep.C.Y - vy;
}
#endregion
#region IDisposable Members
public bool IsDisposed { get; set; }
public void Dispose()
{
if (!IsDisposed)
{
World.RemoveBody(this);
IsDisposed = true;
GC.SuppressFinalize(this);
}
}
#endregion
}
}
相比前一个版本的Body类，主要有以下变化：
1．添加了BodyType枚举
如果Body的类型只有静态和动态两种的话，那么使用bool值IsStatic就完全够用了，设置BodyType是多此一举，但是以后还会添加第三种状态，所以需要这个枚举。
2．添加了BodyFlags标志
如果一个类中有很多bool变量，比较好的组织形式是创建一个位标志变量。一个位标志中的一个位有0和1两种状态，正好对应true和false。以BodyFlag中的IgnoreGravity位为例，代码IgnoreGravity = (1 << 1)表示IgnoreGravity对应的是BodyFlag变量的倒数第二个位，要将IgnoreGravity位设置为1，应该使用或运算：
Flags |= BodyFlags.IgnoreGravity;
要将IgnoreGravity位设置为0，应该使用与非运算：
Flags &= ~BodyFlags.IgnoreGravity;
要判断IgnoreGravity位是1还是0，可使用以下代码，如果为1则返回true：
(Flags & BodyFlags.IgnoreGravity) == BodyFlags.IgnoreGravity
这篇文章XNA中的位标志也可供参考。
3．速度和位移的积分运算移至World类中进行处理
这样做可以提高运行速度，在这篇文章http://www.sgtconker.com/2011/05/high-end-performance-optimizations-on-the-xbox-360-and-windows-phone-7/中Manually inlining high frequency提及了这个技巧。
4．世界坐标和本地坐标的变换
首先需要清楚一个事实，即Body的原点O和质心C不一定在同一个点上。原点O是Body的旋转中心，而质心C是由Body的形状和质量分布决定的。例如，一个64×32大小的质量分布均匀的矩形，我们指定它的左上角为转轴，即原点O位于左上角，它的本地坐标为(0,0)；而质心C在板的几何中心上，即本地坐标为(32,16)，如下图所示。
图2 世界坐标和本地坐标的转换
现在我们将这个矩形放置在世界坐标系中的(32,32)，并顺时针旋转30度，此时这个Body的Transform结构中的Position应为(32,32)，旋转矩阵RMatrix应为
如果我想获得质心在世界空间中的坐标，应该调用GetWorldPoint方法，其代码为：
////// 给定本地空间中的点，返回世界空间中的点。 /// /// 本地空间中的点。</param name="localPoint" > ///世界空间中的点。 public Vector2 GetWorldPoint(ref Vector2 localPoint) { return new Vector2(Xf.Position.X + Xf.RMatrix.Col1.X * localPoint.X + Xf.RMatrix.Col2.X * localPoint.Y,Xf.Position.Y + Xf.RMatrix.Col1.Y * localPoint.X + Xf.RMatrix.Col2.Y * localPoint.Y); }
即新坐标为(32+32cos30°-16sin30°,32+32sin30°+16cos30°)=(51.72,,61.86)。
其他的用于坐标变换的代码原理类似不再赘述。
5．转动惯量的平行轴定理
在创建Body时，我们会在下面提及的BodyFactory类中自动计算物体相对于质心的转动惯量。但从图2可以看到，转轴不一定与质心重合，此时的转动惯量就要发生变化。
相关物理知识告诉我们：若有任一轴与过质心的轴平行，且该轴与过质心的轴相距为d，刚体对其转动惯量为I，则有：
I＝IC＋md2
其中IC表示相对通过质心的轴的转动惯量。
这个定理称为平行轴定理。
对于图2的情况，若转轴在质心C处时转动惯量为m(642+322)/12，而当转轴变为左上角O时，转动惯量变为m(642+322)/12+m(322+162)。这也是Body类中的Inertia属性代码的原理：
/// <summary>
/// 获取或设置body相对于本地坐标原点的转动惯量，单位为kg-m^2。
/// </summary>
public float Inertia
{
get { return _inertia + Mass * Vector2.Dot(Sweep.LocalCenter, Sweep.LocalCenter); }
set
{
if (_bodyType != BodyType.Dynamic)
return;
if (value > 0.0f)
{
_inertia = value - Mass * Vector2.Dot(LocalCenter, LocalCenter);
InvInertia = 1.0f / _inertia;
}
}
}
BodyFactory类的变化
在很多游戏环境中，根据形状密度计算质量是很有意义的，这能确保物体有合理和一致的质量，通常我们将物体的密度设为相同的值，这样大的物体就有大的质量，非常直观。当然，在某些情况中，你也可以手动设置质量。因此工厂方法传入的参数由质量mass变为密度density，创建矩形Body的代码如下：
public static Body CreateRectangle(World world, float width, float height, float density, Vector2 position)
{
if (width <= 0)
throw new ArgumentOutOfRangeException("width", "矩形的宽不得小于0");
if (height <= 0)
throw new ArgumentOutOfRangeException("height", "矩形的高不得小于0");
if(density <=0)
throw new ArgumentOutOfRangeException("height", "密度不得小于0");
Body body = CreateBody(world, position);
float area = width * height;
body.Mass = density * area;
body.Inertia = body.Mass * (width * width + height * height) / 12;
return body;
}
World类
将PhysicsSimulator类改名为World，将此类中的Update方法改名为Step，完整代码如下：
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Stun2DPhysics4SL.Common;
namespace Stun2DPhysics4SL.Dynamics
{
public class World
{
// 添加或移除的Body集合，在World的每次Step之前调用，
// 在World的Body集合中添加或移除这两个集合中的对象。
private HashSet<Body> _bodyAddList = new HashSet<Body>();
private HashSet<Body> _bodyRemoveList = new HashSet<Body>();
/// <summary>
/// 如果设置为false，整个模拟器就会停止。
/// </summary>
public bool Enabled = true;
// <summary>
/// 创建一个新<see cref="World"/>类对象。
/// </summary>
private World()
{
BodyList = new List<Body>(32);
}
/// <summary>
/// 创建一个新<see cref="World"/>类对象。
/// </summary>
/// <param name="gravity">重力加速度矢量。</param>
public World(Vector2 gravity) : this()
{
Gravity = gravity;
}
/// <summary>
/// 所有Body共享的重力加速度矢量。
/// </summary>
public Vector2 Gravity;
/// <summary>
/// World中的Body集合。
/// </summary>
public List<Body> BodyList { get; private set; }
/// <summary>
/// 在World中添加一个Body。
/// </summary>
internal void AddBody(Body body)
{
if (!_bodyAddList.Contains(body))
_bodyAddList.Add(body);
}
/// <summary>
/// 在World中移除一个Body。
/// </summary>
/// <param name="body">要移除的Body。</param>
public void RemoveBody(Body body)
{
if (!_bodyRemoveList.Contains(body))
_bodyRemoveList.Add(body);
}
/// <summary>
/// 在每次World的步进中缓存要添加或移除的Body对象。
/// 然后在每次更新前调用这个方法处理添加和移除的Body对象。
/// </summary>
public void ProcessChanges()
{
ProcessAddedBodies();
ProcessRemovedBodies();
}
// 将_bodyAddList集合中的Body对象添加到World的Body集合中
private void ProcessAddedBodies()
{
if (_bodyAddList.Count > 0)
{
foreach (Body body in _bodyAddList)
{
// 添加到World的Body集合中。
BodyList.Add(body);
}
_bodyAddList.Clear();
}
}
// 从World的Body集合中移除_bodyRemoveList中的Body对象
private void ProcessRemovedBodies()
{
if (_bodyRemoveList.Count > 0)
{
foreach (Body body in _bodyRemoveList)
{
// 从World的Body集合中移除Body对象
BodyList.Remove(body);
}
_bodyRemoveList.Clear();
}
}
/// <summary>
/// 以一个时间步进为参数，处理Body集合中所有对象的积分运算。
/// </summary>
/// <param name="dt">物理模拟的时间间隔。</param>
public void Step(float dt)
{
ProcessChanges();
if (dt == 0 || !Enabled)
{
return;
}
// 对速度进行积分运算, 然后对位置进行积分计算。
Solve(dt);
ClearForces();
}
/// <summary>
/// 每次调用Step方法之后需要调用这个方法将力和力矩设置为零。
/// </summary>
public void ClearForces()
{
for (int i = 0; i < BodyList.Count; i++)
{
Body body = BodyList[i];
body.Force = Vector2.Zero;
body.Torque = 0.0f;
}
}
private void Solve(float dt)
{
// 遍历Body数组
for (int i = 0; i < BodyList.Count; i++)
{
Body b = BodyList[i];
if (b.BodyType != BodyType.Dynamic)
{
continue;
}
// 对速度进行积分运算，根据Body的IgnoreGravity属性决定是否添加重力的影响
if (b.IgnoreGravity)
{
b.LinearVelocityInternal.X += dt * (b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += dt * (b.InvMass * b.Force.Y);
b.AngularVelocityInternal += dt * b.InvInertia * b.Torque;
}
else
{
b.LinearVelocityInternal.X += dt * (Gravity.X + b.InvMass * b.Force.X);
b.LinearVelocityInternal.Y += dt * (Gravity.Y + b.InvMass * b.Force.Y);
b.AngularVelocityInternal += dt * b.InvInertia * b.Torque;
}
// 施加影响线速度和角速度的阻力。
// 动力学方程: dv/dt + c * v = 0
// 解方程: v(t) = v0 * exp(-c * t)
// 每个时间步进: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// 泰勒级数展开: // v2 = (1.0f - c * dt) * v1
b.LinearVelocityInternal *= MathUtils.Clamp(1.0f - dt * b.LinearDamping, 0.0f, 1.0f);
b.AngularVelocityInternal *= MathUtils.Clamp(1.0f - dt * b.AngularDamping, 0.0f, 1.0f);
// 对位置进行积分运算
b.Sweep.C.X += dt * b.LinearVelocityInternal.X;
b.Sweep.C.Y += dt * b.LinearVelocityInternal.Y;
b.Sweep.A += dt * b.AngularVelocityInternal;
// 将改变同步到Transform
b.SynchronizeTransform();
}
}
}
}
1．_bodyAddList和_bodyRemoveList的作用
当在Step()方法中进行物理计算时，如果此时你在Body集合中添加或移除一个Body对象，就有可能会引发冲突，因此额外还需要两个HashSet集合保存要添加或移除的Body对象，在每次Step()的一开始都会调用ProcessChanges()方法就缓存在集合中的Body变更到BodyList集合中，这样就避免了程序冲突。
2．阻尼代码的解释
在Step()方法中我们不仅对Body集合中的每个集合对象进行了速度和位置的积分运算，而且还添加了阻力效果。
首先在Body类中添加了线性阻尼LinearDamping和角度阻尼系数AngularDamping。对于线性运动来说，物理知识告诉我们，物体在运动过程中受到的空气阻力在速度较小时与v成正比（斯托克斯公式），在速度较大时与v2成正比，为了简化问题，我们取与v成正比的情况。有微分方程：
dv/dt ＝－Cv
解得：
vt＝v0•e-ct
然后进行泰勒级数展开： vt＝v（1－ct－c2t2/2 ＋……）
忽略高次项，取vt＝v（1－ct）。最后将（1－ct）限制在[0，1]之间。角度阻尼的算法是一样的。
Silverlight代码
Silverlight中的代码变化不大，添加了一些代码可以指定控件的初始大小，自己看源代码吧，偷懒不写了。
点击数字键1、2、3可以分别让三个矩形发生旋转，而且由于阻尼的作用，旋转一段时间就会停止。
文件下载（已下载 1048 次）
发布时间：2011/7/6 0:59:24 阅读次数：5635
