public void AddSegment(float[] p, float[] q)
        {
            if (_nSegments >= _maxSegments)
            {
                return;
            }

            ObstacleSegment seg = _segments[_nSegments++];

            Helper.VCopy(ref seg.p, p);
            Helper.VCopy(ref seg.q, q);
        }
 public bool Init(int maxCircles, int maxSegments)
 {
     _maxCircles = maxCircles;
     _nCircles   = 0;
     _circles    = new ObstacleCircle[_maxCircles];
     for (int i = 0; i < _maxCircles; i++)
     {
         _circles[i] = new ObstacleCircle();
     }
     _maxSegments = maxSegments;
     _nSegments   = 0;
     _segments    = new ObstacleSegment[_maxSegments];
     for (int i = 0; i < _maxSegments; i++)
     {
         _segments[i] = new ObstacleSegment();
     }
     return(true);
 }
        private void Prepare(float[] pos, float[] dvel)
        {
            for (int i = 0; i < _nCircles; i++)
            {
                ObstacleCircle cir = _circles[i];

                float[] pa = pos;
                float[] pb = cir.p;

                float[] orig = new float[3];
                orig[0] = 0;
                orig[1] = 0;

                float[] dv = new float[3];
                cir.dp = Helper.VSub(pb[0], pb[1], pb[2], pa[0], pa[1], pa[2]);
                Helper.VNormalize(ref cir.dp);
                dv = Helper.VSub(cir.dvel[0], cir.dvel[1], cir.dvel[2], dvel[0], dvel[1], dvel[2]);

                float a = Helper.TriArea2D(orig, cir.dp, dv);
                if (a < 0.01f)
                {
                    cir.np[0] = -cir.dp[2];
                    cir.np[2] = cir.dp[0];
                }
                else
                {
                    cir.np[0] = cir.dp[2];
                    cir.np[2] = -cir.dp[0];
                }
            }

            for (int i = 0; i < _nSegments; i++)
            {
                ObstacleSegment seg = _segments[i];

                float r = 0.01f;
                float t;
                seg.touch = Helper.DistancePtSegSqr2D(pos[0], pos[1], pos[2], seg.p[0], seg.p[1], seg.p[2], seg.q[0], seg.q[1], seg.q[2], out t) < r * r;
            }
        }
        private float ProcessSample(float[] vcand, float cs, float[] pos, float rad, float[] vel, float[] dvel, ObstacleAvoidanceDebugData debug = null)
        {
            float tmin  = _params.horizTime;
            float side  = 0;
            int   nside = 0;

            for (int i = 0; i < _nCircles; i++)
            {
                ObstacleCircle cir = _circles[i];

                float[] vab = Helper.VScale(vcand[0], vcand[1], vcand[2], 2);
                vab = Helper.VSub(vab[0], vab[1], vab[2], vel[0], vel[1], vel[2]);

                side += (float)Math.Max(0.0, Math.Min(1.0, Math.Min(Helper.VDot2D(cir.dp, vab) * 0.5f + 0.5f, Helper.VDot2D(cir.np, vab) * 2f)));
                nside++;

                float htmin = 0, htmax = 0;
                if (SweetCircleCircle(pos, rad, vab, cir.p, cir.rad, ref htmin, ref htmax) == 0)
                {
                    continue;
                }

                if (htmin < 0.0f && htmax > 0.0f)
                {
                    htmin = -htmin * 0.5f;
                }

                if (htmin >= 0.0f)
                {
                    if (htmin < tmin)
                    {
                        tmin = htmin;
                    }
                }
            }

            for (int i = 0; i < _nSegments; i++)
            {
                ObstacleSegment seg   = _segments[i];
                float           htmin = 0;

                if (seg.touch)
                {
                    float[] sdir  = Helper.VSub(seg.q[0], seg.q[1], seg.q[2], seg.p[0], seg.p[1], seg.p[2]);
                    float[] snorm = new float[3];
                    snorm[0] = -sdir[2];
                    snorm[2] = sdir[0];

                    if (Helper.VDot2D(snorm, vcand) < 0.0f)
                    {
                        continue;
                    }

                    htmin = 0.0f;
                }
                else
                {
                    if (isectRaySeg(pos, vcand, seg.p, seg.q, ref htmin) == 0)
                    {
                        continue;
                    }
                }

                htmin *= 2.0f;

                if (htmin < tmin)
                {
                    tmin = htmin;
                }
            }

            if (nside > 0)
            {
                side /= nside;
            }

            float vpen  = _params.weightDesVel * (Helper.VDist2D(vcand, dvel) * _invVmax);
            float vcpen = _params.weightCurVel * (Helper.VDist2D(vcand, vel) * _invVmax);
            float spen  = _params.weightSide * side;
            float tpen  = _params.weightToi * (1.0f / (0.1f + tmin * _invHorizTime));

            float penalty = vpen + vcpen + spen + tpen;

            if (debug != null)
            {
                debug.AddSample(vcand, cs, penalty, vpen, vcpen, spen, tpen);
            }

            return(penalty);
        }