LineLine Collision

[petrasvestartas]

Dear Daniel Piker,

I am working with lineline collision on Kangaroo2 implementing RTrees and Spatial Grid for faster collision detection. https://vimeo.com/190852075

All the particles and goals are initialized in a clean manner by indexes, therefore I am using this LineLine Goal constructor:

    public LineLineCollision(int a, int b, int c, int d, double r, double k, bool _show)
    {
        Strength = k;

        PIndex = new int[] { a,b,c,d };
        Move = new Vector3d[4];
        Weighting = new double[4] { k, k, k, k };

        R1 = R2 = r;
        show = _show; //Preview output
    }

Each frame I delete last collision goals and add new ones.

The question I have: How to write a Line Line Goal for calculating all pairs of lines in one goal?

Instead of constructor: public LineLineCollision(int a, int b, int c, int d, double r, double k, bool _show) I want to have a constructor (orginally I have the different constructor but this represent the idea) public LineLineCollision(int[] a, int[] b, int[] c, int[] d, double r, double k, bool _show)

I am wondering how to sum up all the move vector as it should be similar thing to points collision.

Kind Regards, Petras Vestartas

[Dan-Piker]

Hi Petras, Great to see the work you are doing. The indexing does get a bit more complex when you have arrays as inputs like this - have a look at how it is handled here: https://github.com/Dan-Piker/K2Goals/blob/master/Collisions.cs with the ObjectIndices

With a goal like collisions which isn't always active, remember to set the weighting to 0 when things are not colliding. Otherwise, even if the Move vector is (0,0,0), the goal is keeping the point pulled towards its current position which will slow things down slightly.

Also - Simply adding up the move vectors from all the collisions affecting each point will work, but can potentially cause some slight artefacts such as jittering. A better way to do it is to keep count of the number of collisions affecting each point, and divide the move vector by this value after summing up all the vector contributions. Normally this averaging of multiple Move vectors is handled by the solver, but since here we have multiple collisions affecting the same point within one goal we have to handle it within the goal. (don't worry too much about this though, as the effect is small, and several of the existing goals simply use the summing approach)

[petrasvestartas]

Thank you for the reference. I will go through and try to figure out to how to make make the rest of non colliding object to 0. As it was an exact situation you just described about jittering.

Actually I was summing up, all the vectors and before tried to have an array of divisions. I have to go step by step through this to debug the goal:

using System; using System.Collections.Generic; using KangarooSolver; using Rhino.Geometry; using System.Linq; using System.Text; using System.Threading.Tasks; using Plankton; using PlanktonGh; using SpatialSlur; using SpatialSlur.SlurCore; using SpatialSlur.SlurData; using SpatialSlur.SlurRhino;

namespace Pneu_Solver {

public class LineLineCollisionSpatialHash : GoalObject
{
    public double Strength;
    public double R1, R2;
    public bool show = false;
    public Line L1;
    public Line L2;
    public SpatialGrid3d<int> _grid;
    public int[][] _edges;
    public Vec3d[] _positions;
    public Line[] _displayLines;
    public List<Line> _closestLines;
    public int[] _meshIndex;
    public int[] _divisions; //for know how many times vector is added

    public LineLineCollisionSpatialHash(int[][] edges, int[] edgesFlattened, SpatialGrid3d<int> grid, int[] meshIndex, double r, double k, bool _show)
    {
        _edges = edges;
        _positions = new Vec3d[edges.Length];
        _displayLines = new Line[edges.Length];
        _divisions = new int[edgesFlattened.Length]; //for know how many times vector is added
        Strength = k;
        R1 = R2 = r;
        show = _show;
        _closestLines = new List<Line>();
        _grid = grid;
        _meshIndex = meshIndex;

        this.PIndex = edgesFlattened;
        this.Move = new Vector3d[edgesFlattened.Length];
        this.Weighting = new double[edgesFlattened.Length];
        this.Strength = k;
    }

    public override void Calculate(List<KangarooSolver.Particle> p)
    {
        #region Vec3d HalfEdge middle points for SpatialGrid Search
        for (int i = 0; i < _edges.Length; i++)
        {

            _positions[i] = new Vec3d(
                (p[_edges[i][0]].Position.X + p[_edges[i][1]].Position.X) * 0.5,
                (p[_edges[i][0]].Position.Y + p[_edges[i][1]].Position.Y) * 0.5,
                (p[_edges[i][0]].Position.Z + p[_edges[i][1]].Position.Z) * 0.5
                );

            _displayLines[i] = new Line(p[_edges[i][0]].Position, p[_edges[i][1]].Position);
        }
        #endregion

        //SpatialGrid Search
        //Some indices
        _closestLines.Clear();

        //Insert particles into a grid
        for (int i = 0; i < _positions.Length; i++)
            _grid.Insert(_positions[i], i);

        //Search
        double offset = 1;
        for (int i = 0; i < _positions.Length; i++)
        {

                Domain3d bbox = new Domain3d
                (
                    _positions[i] - new Vec3d(0.1 * 2.0, 0.1 * 2.0, 0.1 * 2.0) * offset, //Search radius
                    _positions[i] + new Vec3d(0.1 * 2.0, 0.1 * 2.0, 0.1 * 2.0) * offset //Search radius
                );
                _grid.Search(bbox, foundIds =>
                {
                    foreach (int j in foundIds)
                    {
                        if (j <= i) continue; // only proceed with particles of a higher index to avoid calculating the same collision twice.
                        if (_meshIndex[j] == _meshIndex[i]) continue;
                        //Check if it does not belong to the stripe
                        //Take i and check to what range it belongs
                        //int boundary[]

                        L1 = new Line(p[_edges[i][0]].Position, p[_edges[i][1]].Position);
                        L2 = new Line(p[_edges[j][0]].Position, p[_edges[j][1]].Position);

                        _closestLines.Add(L1);
                        _closestLines.Add(L2);
                        /*
                       // _closestLines.Add(new Line (RhinoExtensions.ToPoint3d(_positions[i]), RhinoExtensions.ToPoint3d(_positions[j]) ));

                        #region calculate goal

                        Vector3d u = L1.To - L1.From;
                        Vector3d v = L2.To - L2.From;
                        Vector3d w = L1.From - L2.From;
                        double a = u * u;
                        double b = u * v;
                        double c = v * v;
                        double d = u * w;
                        double e = v * w;
                        double D = a * c - b * b;
                        double sc, sN, sD = D;
                        double tc, tN, tD = D;

                        // compute the line parameters of the two closest points
                        if (D < 1e-8)
                        {     // the lines are almost parallel
                            sN = 0.0;         // force using point P0 on segment S1
                            sD = 1.0;         // to prevent possible division by 0.0 later
                            tN = e;
                            tD = c;
                        }
                        else
                        {
                            sN = b * e - c * d;
                            tN = a * e - b * d;

                            if (sN < 0.0)
                            {        // sc < 0 => the s=0 edge is visible
                                sN = 0.0;
                                tN = e;
                                tD = c;
                            }
                            else if (sN > sD)
                            {  // sc > 1  => the s=1 edge is visible
                                sN = sD;
                                tN = e + b;
                                tD = c;
                            }
                        }

                        if (tN < 0.0)
                        {            // tc < 0 => the t=0 edge is visible
                            tN = 0.0;
                            // recompute sc for this edge
                            if (-d < 0.0)
                                sN = 0.0;
                            else if (-d > a)
                                sN = sD;
                            else
                            {
                                sN = -d;
                                sD = a;
                            }
                        }
                        else if (tN > tD)
                        {      // tc > 1  => the t=1 edge is visible
                            tN = tD;
                            // recompute sc for this edge
                            if ((-d + b) < 0.0)
                                sN = 0;
                            else if ((-d + b) > a)
                                sN = sD;
                            else
                            {
                                sN = (-d + b);
                                sD = a;
                            }
                        }

                        // finally do the division to get sc and tc
                        sc = (Math.Abs(sN) < 1e-8 ? 0.0 : sN / sD);
                        tc = (Math.Abs(tN) < 1e-8 ? 0.0 : tN / tD);

                        // get the difference of the two closest points
                        Vector3d dP = w + (sc * u) - (tc * v);  // =  S1(sc) - S2(tc)

                        {
                            var Separation = -dP;
                            var Overlap = R1 + R2 - Separation.Length;
                            if (Overlap > 0)
                            {
                                Separation.Unitize();
                                var Push = Separation * Overlap;

                                Move[_edges[i][0]] += (1 - sc) * -Push;
                                Move[_edges[i][1]] += (sc) * -Push;

                                Move[_edges[j][0]] += (1 - tc) * Push;
                                Move[_edges[j][1]] += (tc) * Push;

                                Weighting[_edges[i][0]] = Weighting[_edges[i][1]] = Weighting[_edges[j][0]] = Weighting[_edges[j][1]] = Strength;
                            }
                            else
                            {
                                Move[_edges[i][0]] = Move[_edges[i][1]] = Move[_edges[j][0]] = Move[_edges[j][1]] = Vector3d.Zero;
                                Weighting[_edges[i][0]] = Weighting[_edges[i][1]] = Weighting[_edges[j][0]] = Weighting[_edges[j][1]] = 0;
                            }
                        }
                        #endregion

  */

                    }
                });

        }

        //Clear grid
        _grid.Clear();

    }

public override object Output(List<KangarooSolver.Particle> p)
    {
        return _closestLines;
        //return _displayLines;
    }

}

}

[petrasvestartas]

Hi,

Finally I wrapped linline spatialgrid collision into a goal. It works and meshes collide by naked edges. However I have one more question about overshooting of particles. To go around this issue I changed Spring goal to gradually change its length towards desired value (incrementally adding small steps till desired length). Works but I suspect there is one problem.

In the goal below I am giving PIndex as edges end points in a flattened array of all edges. For instance edgeA is particle 0 and 1 ; edgeB particle 1 and 2.

In the end PIndex array of this goal looks like this where numbers are edge indices 16 0 0 1 1 2 2 3 3 4

when I am looping trough those indices 16 is zero indexed element and 4 is the last member of array. I am wondering if this particular thing may cause overshooting at some rare cases because of averaging in K2 solver.

To illustrate this 0 - 1 PIndices will have their move vectors of some value because they are found by spatialgrid, while not-found 1 - 2 PIndices will remain zero length vectors, 1 Pindex will have 0.5 x of original vector length.

Can this be the case, and maybe you have an idea how to solve overshooting?

Probably there is no way in this example to dynamically change PIndex array of the goal.

Collision goal:

using System;
using System.Collections.Generic;
using System.Linq;
using KangarooSolver;
using Rhino.Geometry;
using SpatialSlur.SlurCore;
using SpatialSlur.SlurData;

namespace Pneu_Solver.SolverPlankton
{
    public class LineLineColliderSpatialHash : GoalObject
    {
        public double Strength;
        public double R1, R2;
        public bool show = false;
        public Line L1;
        public Line L2;

        //Spatial grid
        public int[] _edgesFlattened;
        public SpatialGrid3d<int> _grid;
        public int[] _meshIndex;
        public Vec3d[] _positions;

        //Store found indices for move and weight vectors
        List<int> paI = new List<int>(); //Line a start point
        List<int> pbI = new List<int>(); //Line a endpoint
        List<int> pcI = new List<int>(); //Line b start point
        List<int> pdI = new List<int>(); //Line b endpoint

        public LineLineColliderSpatialHash( int[] edgesFlattened, SpatialGrid3d<int> grid, int[] meshIndex,  double r, double k, bool _show)
        {
            _edgesFlattened = edgesFlattened; //Naked edges end points indices as flattened array
            _grid = grid; //Spatial grid
            _meshIndex = meshIndex;  //Edge index in decomposed meshes. Used to avoid calculating self collisions

            //0 16 0 1 1 2 2 3 3 4 PIndex values array
            //0  1 2 3 4 5 6 7 8 9 PIndex Indices
            //0  0 0 0 0 0 0 0 0 0 Move and Weight array

            PIndex = edgesFlattened;// all edges .___. end points
            Move = new Vector3d[edgesFlattened.Length]; //Initialize all move vector to 0,0,0 for no movement
            Weighting = new double[edgesFlattened.Length]; //Initialize all weights to 0
            _positions = new Vec3d[(int)(edgesFlattened.Length*0.5)];  //Middle points of lines

            Strength = k;
            R1 = R2 = r;
            show = _show; //Boolean Display colliding line
        }

        public override void Calculate(List<KangarooSolver.Particle> p)
        {
            //SpatialGrid
            paI.Clear(); 
            pbI.Clear();
            pcI.Clear();
            pdI.Clear();

            Move = new Vector3d[Move.Length]; //Initialize all move vector to 0,0,0 for no movement
            Weighting = new double[Weighting.Length]; //Initialize all weights to 0

            //Get edges middle point
            for (int i = 0; i < PIndex.Length*0.5; i++)
            {
                _positions[i] = new Vec3d(
                    (p[PIndex[i*2]].Position.X + p[PIndex[i*2+1]].Position.X) * 0.5,
                    (p[PIndex[i*2]].Position.Y + p[PIndex[i*2+1]].Position.Y) * 0.5,
                    (p[PIndex[i*2]].Position.Z + p[PIndex[i*2+1]].Position.Z) * 0.5
                    );
            }

            //Insert particles into the grid
            for (int i = 0; i < _positions.Length; i++)
                _grid.Insert(_positions[i], i);

            //Search
            double offset = 1.5;
            Vec3d size = new Vec3d(0.1*2.0, 0.1*2.0, 0.1*2.0);

            for (int i = 0; i < _positions.Length; i++)
            {
                Domain3d bbox = new Domain3d(_positions[i] - size * offset, _positions[i] + size * offset); //Search radius

                _grid.Search(bbox, foundIds =>
                {     
                    foreach (int j in foundIds)
                    {
                        if (j <= i)
                            continue;

                        if (_meshIndex[j] == _meshIndex[i]) continue; //Check if it does not belong to the stripe

                            //PIndex indices
                            paI.Add(i * 2 + 0);
                            pbI.Add(i * 2 + 1);
                            pcI.Add(j * 2 + 0);
                            pdI.Add(j * 2 + 1);

                    }
                });
            }

            //Clear grid
            _grid.Clear();

            #region math
            //Loop through all found indices
            for (int i = 0; i < paI.Count; i++)
            {
                //Indices used for geometrical caulculation
                //But for move vector we need to track original PIndex particles
                L1 = new Line(p[PIndex[paI[i]]].Position, p[PIndex[pbI[i]]].Position);
                L2 = new Line(p[PIndex[pcI[i]]].Position, p[PIndex[pdI[i]]].Position);

                #region math

                Vector3d u = L1.To - L1.From;
                Vector3d v = L2.To - L2.From;
                Vector3d w = L1.From - L2.From;
                double a = u * u;
                double b = u * v;
                double c = v * v;
                double d = u * w;
                double e = v * w;
                double D = a * c - b * b;
                double sc, sN, sD = D;
                double tc, tN, tD = D;

                // compute the line parameters of the two closest points
                if (D < 1e-8)
                {
                    // the lines are almost parallel
                    sN = 0.0; // force using point P0 on segment S1
                    sD = 1.0; // to prevent possible division by 0.0 later
                    tN = e;
                    tD = c;
                }
                else
                {
                    sN = b * e - c * d;
                    tN = a * e - b * d;

                    if (sN < 0.0)
                    {
                        // sc < 0 => the s=0 edge is visible
                        sN = 0.0;
                        tN = e;
                        tD = c;
                    }
                    else if (sN > sD)
                    {
                        // sc > 1  => the s=1 edge is visible
                        sN = sD;
                        tN = e + b;
                        tD = c;
                    }
                }

                if (tN < 0.0)
                {
                    // tc < 0 => the t=0 edge is visible
                    tN = 0.0;
                    // recompute sc for this edge
                    if (-d < 0.0)
                        sN = 0.0;
                    else if (-d > a)
                        sN = sD;
                    else
                    {
                        sN = -d;
                        sD = a;
                    }
                }
                else if (tN > tD)
                {
                    // tc > 1  => the t=1 edge is visible
                    tN = tD;
                    // recompute sc for this edge
                    if ((-d + b) < 0.0)
                        sN = 0;
                    else if ((-d + b) > a)
                        sN = sD;
                    else
                    {
                        sN = (-d + b);
                        sD = a;
                    }
                }

                // finally do the division to get sc and tc
                sc = (Math.Abs(sN) < 1e-8 ? 0.0 : sN / sD);
                tc = (Math.Abs(tN) < 1e-8 ? 0.0 : tN / tD);

                // get the difference of the two closest points
                Vector3d dP = w + (sc * u) - (tc * v); // =  S1(sc) - S2(tc)

                #endregion

                var Separation = -dP;
                var Overlap = R1 + R2 - Separation.Length; // Move particles only when lines are within the radius

                //Do not forget to sum all weights and move and divide by number of them.

                if (Overlap > 0)
                {
                    Separation.Unitize();
                    var Push = Separation*Overlap;

                    Move[paI[i]] = ((1 - sc)*-Push); 
                    Move[pbI[i]] = ((sc)*-Push); 

                    Move[pcI[i]] = ((1 - tc)*Push); 
                    Move[pdI[i]] = ((tc)*Push); 

                    Weighting[paI[i]] = Weighting[pbI[i]] = Weighting[pcI[i]] = Weighting[pdI[i]] = Strength;
                }
                else
                {
                    Move[paI[i]] = Move[pbI[i]] = Move[pcI[i]] = Move[pdI[i]] = Vector3d.Zero;
                    Weighting[paI[i]] = Weighting[pbI[i]] = Weighting[pcI[i]] = Weighting[pdI[i]] = 0;
                }

            }
            #endregion

        }

        public override object Output(List<KangarooSolver.Particle> p)
        {
            if (show && paI.Count != 0)
            {
                Line[] l = new Line[paI.Count * 2];

                for (int i = 0; i < paI.Count; i++)
                {
                    l[i * 2 + 0] = new Line(p[PIndex[paI[i]]].Position, p[PIndex[pbI[i]]].Position);
                    l[i * 2 + 1] = new Line(p[PIndex[pcI[i]]].Position, p[PIndex[pdI[i]]].Position);
                }
                return l;
            }
            else
            {
                return null;
            }

        }
    }
}