Example #1
0
    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);
                }
            }
        }
    }
Example #2
0
    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;
        }
    }
Example #3
0
    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);
            }

            // 内→外不算,因为线性约束已经把质点拉出物体了
        }
    }