对于速度避障的基本原理不了解的话可以先看 VO避障(动态速度避障)和 RVO:实时多智能体导航的交互速度避障

1.求约束线

考虑A、B两个Agent的情况,沿用之前的思路,我们将问题转化到A的空间坐标系下,此时A位于坐标系的原点,并将A的半径缩放至B上,于是B的位置就变成了(Pb - Pa),半径变成了(Ra + Rb),如下图所示:

在这里插入图片描述
要使A和B在一段时间之后发生碰撞,假设这个时间为t,则A需要在时间t内进入到B的半径范围之内,或者从B的范围内穿过,也就是上图中的阴影部分。

将上图整体除以时间t,即可从空间坐标系,转换为速度坐标系:

在这里插入图片描述
这个速度坐标系有几点需要注意:

  • 这里的速度是B静止,只有A运动时的速度,或者说,是A相对于B的相对运动的速度坐标系。
  • 图中灰色的小圆是第一张图里紫色的圆直接除以 t 转化而来的,灰色的大圆其实并没有什么实际含义,只是根据三角函数可以推断出存在这样一个圆,用于后续计算的,跟上图中紫色的圆并没有实际关系。
  • 灰色阴影部分代表会导致AB在时间 t 内发生碰撞的相对速度,也就是VO区域。
  • 不必纠结小圆和大圆是否相切,因为计算用不到。。。重要的是小圆和大圆的圆心以及原点在一条线上,以及小圆和大圆都与阴影区域的边相切。

跟VO避障一样,ORCA的思路也是Agent在选择速度的时候,只要选择阴影区域以外的值即可避免碰撞。那么当当前的相对速度已经落到阴影区域之内了,要如何选择一个合适的避障速度呢?

在这里插入图片描述
很简单,只要找到最快脱离阴影区域的方向即可,如上图里,红色的点代表当前的相对速度,w向量代表从小圆圆心指向相对速度的向量,u向量代表最快脱离阴影区域的向量,可见,其实只存在两种脱离的情况:

  1. 从小圆的圆弧部分脱离,此时u向量的方向和w向量一致,长度为小圆半径减去w向量的长度
  2. 从阴影部分的两条边上脱离,在圆心连线的右侧就从右侧边脱离,在左侧就从左侧边脱离,u向量的方向垂直于要脱离的边,长度根据三角函数即可求得(假设相对速度为Rv,与脱离边的夹角为α,则u的长度即为:Rv*Sinα)

那么首先需要确定是当前属于上面所说的两种情况中的哪种,因为小圆圆心已知,当前相对速度已知,即w向量已知,因此通过点乘可知w与小圆圆心的朝向关系,这里需要注意的地方:
在这里插入图片描述
w与小圆圆心的点乘小于0并不代表一定在小圆圆弧部分脱离,因为w可能处于上图圆中蓝色的这一部分区域中,我们需要判断的是w完全处于小圆圆心到阴影边的切线的内侧,也就是角α小于角β:
在这里插入图片描述
通过将图中的两个向量做Det运算(我们自定义的一个运算,用于求叉乘结果向量的标量长度,相当于|a||b|Sinθ),即可求得Sinα,通过小圆半径/小圆圆心,即可求得Sinβ,然后通过比较Sin值来确定角度的大小关系。也就是代码里这一段(这里是展开整理过后的):

 				//小圆圆心
                Vector3 _sCircleCenter = _relativePos * mInvTime;
                //小圆半径
                float _sCircleRadius = _combinedRadius * mInvTime;
                Vector3 _w = _relativeVel - _sCircleCenter;
                float _wLenSqr = Vector3.SqrMagnitude(_w);
                float _dot = Vector3.Dot(_w, _sCircleCenter);
                if (_dot < 0 && _dot * _dot > _wLenSqr * _combinedRadius * _combinedRadius)
                {
                    //在圆弧部分计算u,方向与w相同,长度为小圆半径减w的长度
                    ...
                }
                else
                {
                    //在边上计算u
                	...    
                }

在得到u向量之后,将相对速度沿着u的方向进行偏移,只要偏移量超过u的长度,就会脱离阴影区域避免碰撞,或者说存在这么一条与u向量垂直的分割线,只要相对速度偏移到了分割线的另一侧,即可避免碰撞:
在这里插入图片描述
因为可以用 Point + t * Diretion 的方式来表示一条直线,因此要确定这条分割线,需要分别求出Point和Direction。

  • Direction分两种情况:
    1. 在圆弧上脱离时:只要将u旋转90°即可,我这里按个人习惯,是逆时针旋转,即保持线的右侧是允许的速度范围,原版的代码里是顺时针旋转,保持左侧是允许的范围
    2. 在边上脱离时:Line与脱离边平行(看之前u的示意图也能看出来),只是要注意Direction到底是跟边同向还是逆向,最终要保证无论哪种情况,允许的范围都在Line的同一侧。
  • Point很好求,等于RelativeVelocity + u。但是之前说过,这个速度空间是相对速度的空间坐标系,所以这个u向量需要最终分摊给AB两个Agent,所以在实际求point的时候,要乘以一个责任系数,即 Point = RelativeVelocity + weight * u。这个weight在RVO里按照0.5-0.5的方式平均分配,但是这种情况容易造成频繁的抖动,可以通过一些策略,比如右侧担责更大或者高速担责更大,来打破这种平衡性,减少抖动。

至此,对于一个Agent求约束的部分就结束了。

2.获取邻居

对于场景中的多个Agent,通过同样的方式,可以分别求出每个Agent带来的约束线。

当然,并不是场景中的所有Agent都需要求约束线,因为有些Agent距离很远,根本不会发生碰撞,或者即使发生碰撞也在很久之后,不要到在当前时刻参与计算。

所以,在实际求约束线的时候,只需要收集距离比较近的Agent,也就是所谓的邻居,只对这些邻居求约束线即可。

这一部分可以通过K近邻查找或者范围查找来进行,通过KD树或者BVH或者其他空间管理手段都可以,这里提供了K近邻查找和范围查找两种查找邻居的方式,实际测试下来感觉范围查找效果明显要好于K近邻查找。

关于KD树,可以看 使用KD树进行最近邻查找(1NN)和K近邻查找(KNN)

3.动态规划

之前收集到的每条约束线都将空间分成了两个子空间,其中一个为可行子空间,另一个为禁行子空间。动态规划主要负责对所有可行子空间求交集,并从交集中选择离Agent期望速度最近的点作为当帧的实际速度。该过程分为 LinearProgramming_1、LinearProgramming_2、LinearProgramming_3 三步:

  • LinearProgramming_1 是对单条线实际进行求解的计算,如果无解,则范围false,如果有解,则会根据当前线的计算结果对上一步求解的速度进行修正。
  • LinearProgramming_2 负责一次完整的求解,对每条约束线调用LinearProgramming_1,经过LinearProgramming_1的不断修正,得到本次最终的速度。但是有可能存在无解的情况,如果某条线在LinearProgramming_1中无解,表示约束线之间产生了冲突,不存在可以避免和所有邻居发生碰撞的可行速度范围,则会进入LinearProgramming_3。
  • LinearProgramming_3 可以当作是一个异常处理的步骤,当LinearProgramming_2因为无解被迫退出时,代表此时已经无法完全避免碰撞,LinearProgramming_3即从发生无解的那条线开始接着处理剩余的线,此时会放宽约束:通过_dis设定了一个检查的角度或者叫速度违背约束线的偏离程度,每次优先对超过这个偏离程度的约束线求解并更新_dis,来筛选那些更紧急的碰撞,从而做到针对紧急碰撞进行优先避险,得到一个速度的次优解。

这里只对LinearProgramming_1中的计算过程介绍一下,LinearProgramming_2和LinearProgramming_3内容较少就不细嗦了。

在这里插入图片描述
图中蓝色的圆是Agent的最大速度允许的范围,绿色的线是LinearProgramming_1当前正在求解的约束线,P0即Line.Point。P3是Po0到约束线的垂线,即0P3为圆心到约束线的距离。

当蓝色的圆与约束线没有交点时,表示即使最大的速度也无法到达约束线划分出的可行子空间一侧,自然无解。当蓝色的圆与约束线相交时,P1P2之间的这段就是实际的有效速度范围,我们把线的方程代入圆:(Po + t * Direction)(Po + t * Direction) = MaxSpd * MaxSpd,
展开可得到一个关于 t 的二元一次方程:
t * t + 2 * (PoDirection) * t + Po Po - MaxSpd * MaxSpd = 0
=> t * t + 2 * _dotDP * t + _dotPP - MaxSpd * MaxSpd = 0
判别式_delta = b*b - 4 * a * c = _dotDP * _dotDP - _dotPP + _radius * _radius;
如果 _delta < 0,则方程无解,即:

			float _dotDP = Vector3.Dot(_curLine.Direction, _curLine.Point);
            float _dotPP = Vector3.Dot(_curLine.Point, _curLine.Point);
            float _delta = _dotDP * _dotDP - _dotPP + _radius * _radius;
            if (_delta < 0)
            {
                //无解
                return false;
            }

当方程有解时,两个解分别为 -b - sqrt(_delta) 和 -b + sqrt(_delta), 即:

			float _sqrt = Mathf.Sqrt(_delta);
            float _tFar = -_dotDP + _sqrt;
            float _tNear = -_dotDP - _sqrt;

其中 _tNear 为P0到P1的距离,_tFar 为P0到P2的距离。

这样,对于这一条约束线自身的求解就结束了,但是,还需要保证当前线的解依然适用于其他的约束线。由于LinearProgramming_2依次对每条线调用LinearProgramming_1,这里只需要计算前置线与当前线的公共解,后续的约束线在执行LinearProgramming_1时仍然会执行这个步骤,到那时当前线又会参与和后续线求公共解的过程,从而避免遗漏。

当前置线与当前线平行时,根据当前线位于前置线的禁行半平面还是可行半平面,可以直接返回无解或跳过后续计算。
在这里插入图片描述
当前置线和当前线相交时,有如下四种情况:

在这里插入图片描述
从上图可以看出来:
交点位置是穿越前置线可行半平面和禁行半平面的分割点
由于我这里是保持线右侧是可行半平面,因此当_detDD = _curLine.Direction.Det(_preLine.Direction) > 0时,前置线可能会压缩tNear,反之前置线可能会压缩tFar,也就是下面的代码:

 				if (_detDD > 0)
                {
                    _tNear = Mathf.Max(_tNear, _crossT);
                }
                else
                {
                    _tFar = Mathf.Min(_tFar, _crossT);
                }

                if (_tFar < _tNear)
                {
                    return false;
                }

交点crossT的计算就是三角函数,充分利用Det方法替换Sin值,不画图了。

最后,计算最终的_newVel:

  • 当_useOpt为True时,代表从LinearProgramming_3调用进来,此时是紧急避险的速度,因此需要保证避险效果的优先性,因此直接根据避险速度的朝向选择tNear或者tFar。
  • 当_useOpt为False时,代表正常的速度求解,此时会优先选择 tNear 到 tFar 这段区间内更接近 optVel 的速度来尽可能满足Agent移动向目标的期望,所以会求出线段 [tNear, tFar] 上离 optVel 最近的点作为最终速度。
			if (_useOpt)
            {
                if (Vector3.Dot(_optVel, _curLine.Direction) > 0)
                {
                    _newVel = _curLine.Point + _curLine.Direction * _tFar;
                }
                else
                {
                    _newVel = _curLine.Point + _curLine.Direction * _tNear;
                }
            }
            else
            {
                Vector3 _tmp = _optVel - _curLine.Point;
                float _newT = Vector3.Dot(_tmp, _curLine.Direction);
                _newT = Mathf.Clamp(_newT, _tNear, _tFar);
                _newVel = _curLine.Point + _newT * _curLine.Direction;
            }

4.示例代码

完整测试代码如下

using System;
using System.Collections.Generic;
using MapRandom.MapRandomCommon;
using UnityEngine;
using UnityEngine.Serialization;
using Random = UnityEngine.Random;

namespace MapRandom.TestOrca
{
    public class OrcaAgent
    {
        public uint Index;
        public Vector3 Position;
        public Vector3 TargetPos;
        public Vector3 Velocity;
        public Vector3 OptVel;
        public float MaxSpd;
        public float Radius;
        public Transform Trans;

        public float GetWeight(OrcaAgent _other)
        {
            if (Vector3.Dot(Velocity, _other.Velocity) >= 0)
            {
                if ((Position - _other.Position).Det(_other.Velocity) >= 0)
                {
                    return 0.7f;
                }
                return 0.3f;
            }

            if (Mathf.Approximately(Position.GetIndex(0), _other.Position.GetIndex(0)))
            {
                if (Position.GetIndex(1) > _other.Position.GetIndex(1)) return 0.7f;
                return 0.3f;
            }
            if (Position.GetIndex(0) > _other.Position.GetIndex(0))
            {
                return 0.7f;
            }
            return 0.3f;
        }

        public void Clear()
        {
            GameObject.DestroyImmediate(Trans.gameObject);
        }
    }

    public enum EFindNeiborType
    {
        KNN,
        Range,
    }
    
    public class TestORCA : MonoBehaviour
    {
        private struct OrcaLine
        {
            public Vector3 Point;
            public Vector3 Direction;
        }
        
        public int AgentNum = 30;
        public float AgentMaxSpd = 4;
        public float TimeHorizon = 2.5f;
        public float EscapeCollisionTime = 1.5f;
        public Transform AgentRoot;
        public float RandRadius = 30f;
        public EFindNeiborType FindNeiborType = EFindNeiborType.KNN;
        public int NeighborK = 4;
        public float NeighborRange = 10;
        
        
        private TestKDTree mKdTree = new();
        private List<OrcaAgent> mAgentList = new();
        private List<OrcaAgent> mNeighborList = new();
        private List<OrcaLine> mLineList = new();
        private List<OrcaLine> mTmpLineList = new();
        private float mInvTime = 1;

        private void Update()
        {
            if (Input.GetKeyUp(KeyCode.F2))
            {
                mInvTime = 1f / TimeHorizon;
                CreateAgents();
                
            }
            DoMove();
        }

        private void CreateOneAgent(Vector3 _pos, int _index)
        {
            GameObject _agentObj = new GameObject($"Index_{_index}");
            Transform _trans = _agentObj.transform;
            _trans.parent = AgentRoot;
            _trans.position = _pos;
            OrcaAgent _agent = new OrcaAgent();
            _agent.Index = (uint)_index;
            _agent.Position = _pos;
            _agent.TargetPos = -_pos;
            _agent.MaxSpd = AgentMaxSpd;
            _agent.Radius = 1;
            _agent.Trans = _trans;
            mAgentList.Add(_agent);
        }

        private void CreateAgents()
        {
            foreach (var _agent in mAgentList) _agent.Clear();
            mAgentList.Clear();

            float _radPer = 2 * Mathf.PI / AgentNum;
            for (int i = 0; i < AgentNum; i++)
            {
                float _x = RandRadius * Mathf.Cos(_radPer * (i - 1));
                float _y = RandRadius * Mathf.Sin(_radPer * (i - 1));
                Vector3 _pos = new Vector3(_x, _y, 0);
                CreateOneAgent(_pos, i);
            }
        }

        private void DoMove()
        {
            if (0 == mAgentList.Count ) return;
            
            mKdTree.BuildTree(mAgentList);
            
            foreach (OrcaAgent _agent in mAgentList)
            {
                //Calc OptVelocity
                _agent.OptVel = Vector3.Normalize(_agent.TargetPos - _agent.Position);
            }

            for (int i = 0; i < mAgentList.Count; i++)
            {
                //Calc Frame Real Velocity
                CalcAgentVelocity(i);
            }

            for (int i = 0; i < mAgentList.Count; i++)
            {
                OrcaAgent _agent = mAgentList[i];
                Vector3 _newPos = _agent.Position + _agent.Velocity * 0.034f;
                _agent.Position = _newPos;
                _agent.Trans.position = _newPos;   
            }
        }

        private void CalcAgentVelocity(int _selfIndex)
        {
            OrcaAgent _agent = mAgentList[_selfIndex];
            mLineList.Clear();
            
            //Find Neighbor
            mNeighborList.Clear();
            if (FindNeiborType == EFindNeiborType.Range)
            {
                mKdTree.FindRange(_agent, _agent.MaxSpd + _agent.Radius, mNeighborList);
            }
            else if (FindNeiborType == EFindNeiborType.KNN)
            {
                mKdTree.FindKNearestPoint(mNeighborList, _agent.Position, NeighborK);   
            }
            
            for (int i = 0; i < mNeighborList.Count; i++)
            {
                CalcNeighborLine(_selfIndex, i);
            }
            CalcVelocity(_selfIndex);
        }

        /// <summary>
        /// 收集每个Neighbor的半平面
        /// </summary>
        /// <param name="_selfIndex"></param>
        /// <param name="_neighborIndex"></param>
        private void CalcNeighborLine(int _selfIndex, int _neighborIndex)
        {
            OrcaLine _line = new OrcaLine();
            Vector3 _u = Vector3.zero;
            
            OrcaAgent _selfAgent = mAgentList[_selfIndex];
            OrcaAgent _neighborAgent = mNeighborList[_neighborIndex];
            float _combinedRadius = _selfAgent.Radius + _neighborAgent.Radius;
            float _combinedRadiusSqr = _combinedRadius * _combinedRadius;
            Vector3 _relativePos =  _neighborAgent.Position - _selfAgent.Position;
            Vector3 _relativeVel = _selfAgent.OptVel - _neighborAgent.OptVel;
            
            float _relPosLenSqr = Vector3.SqrMagnitude(_relativePos);
            if (_relPosLenSqr < _combinedRadiusSqr)
            {
                //已经发生碰撞时,直接指定脱离的步数
                float _invEscapeStep = 1f / EscapeCollisionTime;
                //小圆圆心
                Vector3 _sCircleCenter = _relativePos * _invEscapeStep;
                //小圆半径
                float _sCircleRadius = _combinedRadius * _invEscapeStep;
                Vector3 _w = _relativeVel - _sCircleCenter;
                float _wLenSqr = Vector3.SqrMagnitude(_w);
                float _wLen = (float)Math.Sqrt(_wLenSqr);
                _line.Direction = new Vector3(-_w.y, _w.x) * _invEscapeStep;
                _u = (_sCircleRadius * _invEscapeStep - _wLen) * _w * _invEscapeStep;
            }
            else
            {
                //还未发生碰撞
                //小圆圆心
                Vector3 _sCircleCenter = _relativePos * mInvTime;
                //小圆半径
                float _sCircleRadius = _combinedRadius * mInvTime;
                Vector3 _w = _relativeVel - _sCircleCenter;
                float _wLenSqr = Vector3.SqrMagnitude(_w);
                float _dot = Vector3.Dot(_w, _sCircleCenter);
                if (_dot < 0 && _dot * _dot > _wLenSqr * _combinedRadius * _combinedRadius)
                {
                    //在圆弧部分计算u,方向与w相同,长度为小圆半径减w的长度
                    float _wLen = (float)Math.Sqrt(_wLenSqr);
                    float _invLenW = 1 / _wLen;
                    _line.Direction = new Vector3(-_w.y, _w.x) * _invLenW;
                    _u = (_sCircleRadius - _wLen) * _w * _invLenW;
                }
                else
                {
                    //在边上计算u
                    float _legLen = Mathf.Sqrt(_relPosLenSqr - _combinedRadiusSqr);
                    if (_w.Det(_relativePos) >= 0)
                    {
                        //右侧的边
                        _line.Direction = new Vector3(_legLen * _relativePos.x + _combinedRadius * _relativePos.y,
                            -_combinedRadius * _relativePos.x + _legLen * _relativePos.y) / _relPosLenSqr;
                    }
                    else
                    {
                        //左侧的边
                        _line.Direction = -new Vector3(_legLen * _relativePos.x - _combinedRadius * _relativePos.y,
                            _combinedRadius * _relativePos.x + _legLen * _relativePos.y) / _relPosLenSqr;
                    }
                    _u = _line.Direction * Vector3.Dot(_relativeVel, _line.Direction) - _relativeVel;
                }
            }
            
            _line.Point = _selfAgent.Velocity + _selfAgent.GetWeight(_neighborAgent) * _u;
            mLineList.Add(_line);
        }

        /// <summary>
        /// 通过三步线性规划计算当帧的实际速度
        /// </summary>
        /// <param name="_selfIndex"></param>
        private void CalcVelocity(int _selfIndex)
        {
            OrcaAgent _selfAgent = mAgentList[_selfIndex];
            Vector3 _newVel = _selfAgent.OptVel;
            int _blockLineIndex = LinearProgramming_2(mLineList, _selfIndex, _selfAgent.OptVel, false, ref _newVel);
            if (_blockLineIndex < mLineList.Count)
            {
                //求解失败
                LinearProgramming_3(_blockLineIndex, ref _newVel);
            }
            _selfAgent.Velocity = _selfAgent.Velocity * 0.7f + _newVel * 0.3f;
        }

        private bool LinearProgramming_1(int _selfIndex, float _radius, in List<OrcaLine> _lines, int _curLineIndex, Vector3 _optVel, bool _useOpt, ref Vector3 _newVel)
        {
            OrcaLine _curLine = _lines[_curLineIndex];
            
            float _dotDP = Vector3.Dot(_curLine.Direction, _curLine.Point);
            float _dotPP = Vector3.Dot(_curLine.Point, _curLine.Point);
            float _delta = _dotDP * _dotDP - _dotPP + _radius * _radius;
            if (_delta < 0)
            {
                //无解
                return false;
            }

            float _sqrt = Mathf.Sqrt(_delta);
            float _tFar = -_dotDP + _sqrt;
            float _tNear = -_dotDP - _sqrt;

            //需要保证满足前置线的约束
            for (int i = 0; i < _curLineIndex; i++)
            {
                OrcaLine _preLine = _lines[i];
                float _detDD = _curLine.Direction.Det(_preLine.Direction);
                float _detPD = (_curLine.Point - _preLine.Point).Det(_preLine.Direction);
                
                //两条线平行的情况,需要看是在左侧还是右侧
                if (Mathf.Approximately(_detDD, 0))
                {
                    if (_detPD > 0)
                    {
                        //当前线在前置线的右侧,前置线可以跳过检查
                        continue;
                    }
                    else
                    {
                        //当前线在前置线的左侧,完全被限制,无解
                        return false;
                    }
                }
                
                //两条线相交的情况,根据交点来进行限制
                float _crossT = _detPD / _detDD;
                if (_detDD > 0)
                {
                    _tNear = Mathf.Max(_tNear, _crossT);
                }
                else
                {
                    _tFar = Mathf.Min(_tFar, _crossT);
                }

                if (_tFar < _tNear)
                {
                    return false;
                }
            }

            if (_useOpt)
            {
                if (Vector3.Dot(_optVel, _curLine.Direction) > 0)
                {
                    _newVel = _curLine.Point + _curLine.Direction * _tFar;
                }
                else
                {
                    _newVel = _curLine.Point + _curLine.Direction * _tNear;
                }
            }
            else
            {
                Vector3 _tmp = _optVel - _curLine.Point;
                float _newT = Vector3.Dot(_tmp, _curLine.Direction);
                _newT = Mathf.Clamp(_newT, _tNear, _tFar);
                _newVel = _curLine.Point + _newT * _curLine.Direction;
            }
            
            return true;
        }

        private int LinearProgramming_2(in List<OrcaLine> _lines, int _selfIndex, Vector3 _optVel, bool _useOpt, ref Vector3 _newVel)
        {
            OrcaAgent _selfAgent = mAgentList[_selfIndex];
            if (_useOpt)
            {
                _newVel = _optVel * _selfAgent.MaxSpd;
            }
            else if (Vector3.SqrMagnitude(_newVel) > _selfAgent.MaxSpd * _selfAgent.MaxSpd)
            {
                _newVel = _newVel.normalized * _selfAgent.MaxSpd;
            }
            else
            {
                _newVel = _optVel;
            }
            
            for (int i = 0; i < _lines.Count; i++)
            {
                OrcaLine _curLine = _lines[i];
                if ((_newVel - _curLine.Point).Det(_curLine.Direction) < 0)
                {
                    Vector3 _tmp = _newVel;
                    if (!LinearProgramming_1(_selfIndex, _selfAgent.MaxSpd, _lines, i, _selfAgent.OptVel, _useOpt, ref _newVel))
                    {
                        //需要还原回失败前的速度,因为这条线会在LinearProgramming_3中被重新处理
                        _newVel = _tmp;
                        return i;
                    }
                }
            }
            return _lines.Count;
        }

        private void LinearProgramming_3(int _lineIndex, ref Vector3 _newVel)
        {
            float _dis = 0;
            for (int i = _lineIndex; i < mLineList.Count; i++)
            {
                OrcaLine _curLine = mLineList[i];
                if ((_newVel - _curLine.Point).Det(_curLine.Direction) > _dis) continue;
                
                mTmpLineList.Clear();
                for (int j = 0; j < i; j++)
                {
                    OrcaLine _preLine = mLineList[j];
                    OrcaLine _newLine = new OrcaLine();
                    float _detDD = _curLine.Direction.Det(_preLine.Direction);
                    if (Mathf.Approximately(_detDD, 0))
                    {
                        if (Vector3.Dot(_curLine.Direction, _preLine.Direction) > 0)
                        {
                            continue;
                        }
                        else
                        {
                            _newLine.Point = (_curLine.Point + _preLine.Point) * 0.5f;
                        }
                    }
                    else
                    {
                        float _detPD = (_curLine.Point - _preLine.Point).Det(_preLine.Direction);
                        _newLine.Point = _curLine.Point + _detPD / _detDD * _curLine.Direction;
                    }
                    Vector3 _tmp = _curLine.Direction - _preLine.Direction;
                    if (Mathf.Approximately(Vector3.SqrMagnitude(_tmp), 0))
                    {
                        //同向,无法Normalize,直接用当前线的方向
                        _newLine.Direction = _curLine.Direction;
                    }
                    else
                    {
                        //不同向,
                        _newLine.Direction = Vector3.Normalize(_tmp);
                    }
                    mTmpLineList.Add(_newLine);
                }
                
                Vector3 _newOpt = new Vector3(_curLine.Direction.y, -_curLine.Direction.x);
                LinearProgramming_2(mTmpLineList, i, _newOpt, true, ref _newVel);
                _dis = (_newVel - _curLine.Point).Det(_curLine.Direction);
                // Debug.Log($"LinearProgramming_3 false _lineIndex : {_lineIndex} _newVel : {_newVel}");
            }
        }

    }
}

测试效果如下:
在这里插入图片描述

Logo

北京人形旗下天工造物具身智能开源社区,聚焦具身天工与慧思开物两大平台

更多推荐