private void NarrowPhaseWithOBB()
{
m_rigidContactInfos.Clear();
for (int i = 0; i < m_colliders.Count; i++)
{
Collider cld = m_colliders[i];
Rigidbody rigid = cld.attachedRigidbody;
if (rigid == null)
{
continue;
}
Transform tr = cld.transform;
OBB obb = new OBB(tr);
for (int j = 0; j < m_nodes.Count; j++)
{
Node node = m_nodes[j];
Vector3 point;
// float dst = obb.MinDistanceWhenPntInOBB(node.curpos, out normal);
float dst = obb.ClostestPntOnOBB(node.curpos, out point);
if (dst <= 0)
{
Vector3 normal = (point - node.curpos).normalized;
Vector3 ra = node.curpos - tr.position;
Vector3 va = rigid.velocity + Vector3.Cross(rigid.angularVelocity, ra);
va *= Time.fixedDeltaTime;
Vector3 vb = node.curpos - node.prevpos;
Vector3 vr = vb - va;
// dn相当于相对速度的法相分量
float dn = Vector3.Dot(vr, normal);
// fv相当于相对速度的切线分量
Vector3 fv = vr - normal * dn;
RigidContactInfo rci = new RigidContactInfo();
rci.hardness = Hardness;
rci.friction = fv.sqrMagnitude < (dn * Friction * dn * Friction) ? 0 : 1 - Friction;
rci.normal = normal;
rci.closestPnt = point;
rci.node = node;
rci.collider = cld;
rci.param0 = node.w * Time.fixedDeltaTime;
rci.param1 = ra;
rci.impMat = ImpulseMatrix(node.w, 1.0f / obb.mass, obb.InvInertiaTensor(tr.rotation), ra);
m_rigidContactInfos.Add(rci);
}
}
}
}
protected void SolveContraints()
{
// solve position constraints
for (int i = 0; i < INTERATIONS; i++)
{
// contacts solver
for (int j = 0; j < m_rigidContactInfos.Count; j++)
{
RigidContactInfo rci = m_rigidContactInfos[j];
Node node = rci.node;
Collider collider = rci.collider;
Rigidbody rigid = null;
Vector3 va = Vector3.zero;
if (collider != null)
{
// 刚体的速度
rigid = collider.attachedRigidbody;
if (rigid != null)
{
va = rigid.velocity + Vector3.Cross(rigid.angularVelocity, rci.param1);
va *= Time.fixedDeltaTime;
}
}
Vector3 vb = node.curpos - node.prevpos;
Vector3 vr = vb - va;
float dn = Vector3.Dot(vr, rci.normal);
if (dn <= 2.2204460492503131e-016)
{
float dp = Mathf.Min(-Vector3.Distance(rci.closestPnt, node.curpos), -0.025f);
Vector3 fv = vr - rci.normal * dn;
Vector3 impulse = rci.impMat * (vr - (fv * rci.friction) + (rci.normal * dp * rci.hardness));
node.curpos -= impulse * rci.param0;
if (rigid != null)
{
// 刚体收到的冲量
rigid.AddForceAtPosition(impulse, node.curpos, ForceMode.Impulse);
}
}
}
// linear soler
for (int j = 0; j < m_links.Count; j++)
{
Link link = m_links[j];
if (link.param0 > 0)
{
Node n1 = link.n1;
Node n2 = link.n2;
Vector3 dir = n1.curpos - n2.curpos;
float len = dir.sqrMagnitude;
if (Mathf.Abs(len - link.param1) > 1.192092896e-07F)
{
Vector3 dp = (len - link.param1) / (len + link.param1) / link.param0 * dir;
n1.curpos -= n1.w * dp;
n2.curpos += n2.w * dp;
}
}
}
// volume solver
if (VolumePerserve > 0)
{
float CX = 0.0f;
float W = 0.0f;
for (int j = 0; j < m_nodes.Count; j++)
{
Node node = m_nodes[j];
CX += Vector3.Dot(node.curpos, node.CalcAreaWeightedNormal());
W += node.w * node.areaWeightedNormal.magnitude;
}
CX = CX / 3.0f - m_3v;
W = 3.0f * VolumePerserve / W;
for (int j = 0; j < m_nodes.Count; j++)
{
Node node = m_nodes[j];
node.curpos -= node.w * W * CX * node.areaWeightedNormal;
}
}
}
// update velocity
for (int i = 0; i < m_nodes.Count; i++)
{
Node node = m_nodes[i];
node.velocity = (node.curpos - node.prevpos) / Time.fixedDeltaTime;
}
}
private void NarrowPhaseWithTerrain()
{
for (int i = 0; i < m_nodes.Count; i++)
{
// 检测各个顶点是否与collider碰撞。不精确，有时三个点不在collider内，但是三角形却与collider相交
Node node = m_nodes[i];
RaycastHit hit;
int layerMask = 1 << LayerMask.NameToLayer("Terrain");
RigidContactInfo rci = new RigidContactInfo();
// 检测一下质点是否在物体内，因为上一帧的impulse不一定能把质点移出到物体外，这时射线是不能喝物体相交的
float y = m_terrain.SampleHeight(node.curpos);
if (node.curpos.y < y)
{
if (node.prevpos.y < y)
{
// 上一帧的impulse没把质点移出物体外
// 这样算距离太暴力了，应该算当前位置和网格的最近距离，但是TerrainCollider不支持Physics.ClosetPoint
// float dst = node.curpos.y - y;
// 可以通过梯度下降法求一个近似的最近点。限制最多迭代次数
Vector3 closetPoint;
GradientDescent(m_terrain, node.curpos, out closetPoint);
if (Physics.Raycast(node.curpos, closetPoint - node.curpos, out hit, layerMask))
{
rci.normal = hit.normal;
}
rci.node = node;
rci.closestPnt = closetPoint;
rci.collider = null;
rci.param0 = node.w * Time.fixedDeltaTime;
rci.impMat = ImpulseMatrix(node.w, 0, Matrix4x4.zero, node.curpos);
m_rigidContactInfos.Add(rci);
}
else if (Physics.Linecast(node.prevpos, node.curpos, out hit, layerMask))
{
// 外→内
float dst = -(node.curpos - hit.point).magnitude;
rci.normal = hit.normal.normalized;
rci.node = node;
rci.collider = null;
rci.param0 = node.w * Time.fixedDeltaTime;
rci.closestPnt = node.curpos - rci.normal * dst;
rci.impMat = ImpulseMatrix(node.w, 0, Matrix4x4.zero, node.curpos);
m_rigidContactInfos.Add(rci);
}
}
else if (node.prevpos.y >= y && Physics.Linecast(node.prevpos, node.curpos, out hit, layerMask))
{
// 都在物体外，但射线穿过物体，小物体、尖锐的地方可能会出现这种情况
float dst = -(node.curpos - hit.point).magnitude;
rci.normal = hit.normal.normalized;
rci.node = node;
rci.collider = null;
rci.param0 = node.w * Time.fixedDeltaTime;
rci.closestPnt = node.curpos - rci.normal * dst;
rci.impMat = ImpulseMatrix(node.w, 0, Matrix4x4.zero, node.curpos);
m_rigidContactInfos.Add(rci);
}
// 内→外不算，因为线性约束已经把质点拉出物体了
}
}
