Search Unity

  1. Megacity Metro Demo now available. Download now.
    Dismiss Notice
  2. Unity support for visionOS is now available. Learn more in our blog post.
    Dismiss Notice

Get the collision points in Physics.OverlapSphere

Discussion in 'Physics' started by Moddingear, Apr 1, 2016.

  1. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    Hey.
    It looks like with Physics.OverlapSphere, you only get the colliders, not the collision points in world space like SphereCast do.
    So is there a way to do it ?
    Also, just the distance could be enough for me.
     
  2. Hyblademin

    Hyblademin

    Joined:
    Oct 14, 2013
    Posts:
    725
    If distance is good enough, then use Vector3.Distance() to get the distance between the center of the sphere that was checked, and the Collider.transform.position for each collider found. You won't get distance to actual contact points, of course, but does SphereCast even do this?

    I think I remember from another thread that SphereCast actually returns the position of the center of the sphere at the time it intersects something, not the actual contact point. I could be wrong.
     
  3. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    Yeah, but I use Overlap Sphere when my SphereCast is false, and Vector3.Distance() starts from the origin of the collided object.
    SphereCast doesn't works when the objects are too close, so I thinked about using a raycast when SphereCast is false, but it doesn't works as well when the object is not at the center.

    So I need a way to do a SphereCast that can get everything everything inside, or the closest thing inside and the point at wich it collided.

    (I'm working with that to make a hover)
     
  4. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
  5. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
  6. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    Why is it heavy calculations? What do you think the physics engine does when you are spherecasting and such ^_~

    I have not profiled the performance, but I dont think it would be bad. Its a overlapsphere + closestpoint calculation. Since the mesh colliders will use the BSPTree, it makes it a lot faster.

    You might have to edit the bsptree to make it garbage free though (reusing lists and no foreach).
     
  7. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    thanks, i'll try this.
     
  8. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    This script works like S***.
    I got >0.1fps, so I think i'll multi raycast.
    Anyways, if anyone else have a better script, i'll take it.
    Here's the code I modified :
    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3. using System.Collections.Generic;
    4.  
    5. /// <summary>
    6. /// Recursively paritions a mesh's vertices to allow to more quickly
    7. /// narrow down the search for a nearest point on it's surface with respect to another
    8. /// point
    9. /// </summary>
    10. [RequireComponent(typeof(MeshCollider))]
    11. public class BSPTree : MonoBehaviour
    12. {
    13.  
    14.     [SerializeField]
    15.     bool drawMeshTreeOnStart;
    16.  
    17.     public class Node
    18.     {
    19.         public Vector3 partitionPoint;
    20.         public Vector3 partitionNormal;
    21.  
    22.         public Node positiveChild;
    23.         public Node negativeChild;
    24.  
    25.         public int[] triangles;
    26.     };
    27.  
    28.     private int triangleCount;
    29.     private int vertexCount;
    30.     private Vector3[] vertices;
    31.     private int[] tris;
    32.     private Vector3[] triangleNormals;
    33.  
    34.     private Mesh mesh;
    35.  
    36.     private Node tree;
    37.  
    38.     void Awake()
    39.     {
    40.         mesh = GetComponent<MeshCollider>().sharedMesh;
    41.  
    42.         tris = mesh.triangles;
    43.         vertices = mesh.vertices;
    44.  
    45.         vertexCount = mesh.vertices.Length;
    46.         triangleCount = mesh.triangles.Length / 3;
    47.  
    48.         triangleNormals = new Vector3[triangleCount];
    49.  
    50.         for (int i = 0; i < tris.Length; i += 3)
    51.         {
    52.             Vector3 normal = Vector3.Cross((vertices[tris[i + 1]] - vertices[tris[i]]).normalized, (vertices[tris[i + 2]] - vertices[tris[i]]).normalized).normalized;
    53.  
    54.             triangleNormals[i / 3] = normal;
    55.         }
    56.  
    57.         if (!drawMeshTreeOnStart)
    58.             BuildTriangleTree();
    59.     }
    60.  
    61.     void Start()
    62.     {
    63.         if (drawMeshTreeOnStart)
    64.             BuildTriangleTree();
    65.     }
    66.  
    67.     /// <summary>
    68.     /// Returns the closest point on the mesh with respect to Vector3 point to
    69.     /// </summary>
    70.     public Vector3 ClosestPointOn(Vector3 to, float radius)
    71.     {
    72.         to = transform.InverseTransformPoint(to);
    73.  
    74.         List<int> triangles = new List<int>();
    75.  
    76.         FindClosestTriangles(tree, to, radius, triangles);
    77.  
    78.         Vector3 closest = ClosestPointOnTriangle(triangles.ToArray(), to);
    79.  
    80.         return transform.TransformPoint(closest);
    81.     }
    82.  
    83.     void FindClosestTriangles(Node node, Vector3 to, float radius, List<int> triangles)
    84.     {
    85.         if (node.triangles == null)
    86.         {
    87.             if (PointDistanceFromPlane(node.partitionPoint, node.partitionNormal, to) <= radius)
    88.             {
    89.                 FindClosestTriangles(node.positiveChild, to, radius, triangles);
    90.                 FindClosestTriangles(node.negativeChild, to, radius, triangles);
    91.             }
    92.             else if (PointAbovePlane(node.partitionPoint, node.partitionNormal, to))
    93.             {
    94.                 FindClosestTriangles(node.positiveChild, to, radius, triangles);
    95.             }
    96.             else
    97.             {
    98.                 FindClosestTriangles(node.negativeChild, to, radius, triangles);
    99.             }
    100.         }
    101.         else
    102.         {
    103.             triangles.AddRange(node.triangles);
    104.         }
    105.     }
    106.  
    107.     Vector3 ClosestPointOnTriangle(int[] triangles, Vector3 to)
    108.     {
    109.         float shortestDistance = float.MaxValue;
    110.  
    111.         Vector3 shortestPoint = Vector3.zero;
    112.  
    113.         // Iterate through all triangles
    114.         foreach (int triangle in triangles)
    115.         {
    116.             Vector3 p1 = vertices[tris[triangle]];
    117.             Vector3 p2 = vertices[tris[triangle + 1]];
    118.             Vector3 p3 = vertices[tris[triangle + 2]];
    119.  
    120.             Vector3 nearest;
    121.  
    122.             ClosestPointOnTriangleToPoint(ref p1, ref p2, ref p3, ref to, out nearest);
    123.  
    124.             float distance = (to - nearest).sqrMagnitude;
    125.  
    126.             if (distance <= shortestDistance)
    127.             {
    128.                 shortestDistance = distance;
    129.                 shortestPoint = nearest;
    130.             }
    131.         }
    132.  
    133.         return shortestPoint;
    134.     }
    135.  
    136.     void BuildTriangleTree()
    137.     {
    138.         List<int> rootTriangles = new List<int>();
    139.  
    140.         for (int i = 0; i < tris.Length; i += 3)
    141.         {
    142.             rootTriangles.Add(i);
    143.         }
    144.  
    145.         tree = new Node();
    146.  
    147.         RecursivePartition(rootTriangles, 0, tree);
    148.     }
    149.  
    150.     void RecursivePartition(List<int> triangles, int depth, Node parent)
    151.     {
    152.         Vector3 partitionPoint = Vector3.zero;
    153.  
    154.         Vector3 maxExtents = new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue);
    155.         Vector3 minExtents = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
    156.  
    157.         foreach (int triangle in triangles)
    158.         {
    159.             partitionPoint += vertices[tris[triangle]] + vertices[tris[triangle + 1]] + vertices[tris[triangle + 2]];
    160.  
    161.             minExtents.x = Mathf.Min(minExtents.x, vertices[tris[triangle]].x, vertices[tris[triangle + 1]].x, vertices[tris[triangle + 2]].x);
    162.             minExtents.y = Mathf.Min(minExtents.y, vertices[tris[triangle]].y, vertices[tris[triangle + 1]].y, vertices[tris[triangle + 2]].y);
    163.             minExtents.z = Mathf.Min(minExtents.z, vertices[tris[triangle]].z, vertices[tris[triangle + 1]].z, vertices[tris[triangle + 2]].z);
    164.  
    165.             maxExtents.x = Mathf.Max(maxExtents.x, vertices[tris[triangle]].x, vertices[tris[triangle + 1]].x, vertices[tris[triangle + 2]].x);
    166.             maxExtents.y = Mathf.Max(maxExtents.y, vertices[tris[triangle]].y, vertices[tris[triangle + 1]].y, vertices[tris[triangle + 2]].y);
    167.             maxExtents.z = Mathf.Max(maxExtents.z, vertices[tris[triangle]].z, vertices[tris[triangle + 1]].z, vertices[tris[triangle + 2]].z);
    168.         }
    169.  
    170.         // Centroid of all vertices
    171.         partitionPoint /= vertexCount;
    172.  
    173.         // Better idea? Center of bounding box
    174.         partitionPoint = minExtents + (Vector3.Normalize(maxExtents - minExtents) * (maxExtents - minExtents).magnitude * 0.5f);
    175.  
    176.         Vector3 extentsMagnitude = new Vector3(Mathf.Abs(maxExtents.x - minExtents.x), Mathf.Abs(maxExtents.y - minExtents.y), Mathf.Abs(maxExtents.z - minExtents.z));
    177.  
    178.         Vector3 partitionNormal;
    179.  
    180.         if (extentsMagnitude.x >= extentsMagnitude.y && extentsMagnitude.x >= extentsMagnitude.z)
    181.             partitionNormal = Vector3.right;
    182.         else if (extentsMagnitude.y >= extentsMagnitude.x && extentsMagnitude.y >= extentsMagnitude.z)
    183.             partitionNormal = Vector3.up;
    184.         else
    185.             partitionNormal = Vector3.forward;
    186.  
    187.         List<int> positiveTriangles;
    188.         List<int> negativeTriangles;
    189.  
    190.         Split(triangles, partitionPoint, partitionNormal, out positiveTriangles, out negativeTriangles);
    191.  
    192.         parent.partitionNormal = partitionNormal;
    193.         parent.partitionPoint = partitionPoint;
    194.  
    195.         Node posNode = new Node();
    196.         parent.positiveChild = posNode;
    197.  
    198.         Node negNode = new Node();
    199.         parent.negativeChild = negNode;
    200.  
    201.         if (positiveTriangles.Count < triangles.Count && positiveTriangles.Count > 3)
    202.         {
    203.             RecursivePartition(positiveTriangles, depth + 1, posNode);
    204.         }
    205.         else
    206.         {
    207.             posNode.triangles = positiveTriangles.ToArray();
    208.  
    209.             if (drawMeshTreeOnStart)
    210.                 DrawTriangleSet(posNode.triangles, Color.yellow);
    211.         }
    212.  
    213.         if (negativeTriangles.Count < triangles.Count && negativeTriangles.Count > 3)
    214.         {
    215.             RecursivePartition(negativeTriangles, depth + 1, negNode);
    216.         }
    217.         else
    218.         {
    219.             negNode.triangles = negativeTriangles.ToArray();
    220.  
    221.             if (drawMeshTreeOnStart)
    222.                 DrawTriangleSet(negNode.triangles, Color.cyan);
    223.         }
    224.  
    225.     }
    226.  
    227.     /// <summary>
    228.     /// Splits a a set of input triangles by a partition plane into positive and negative sets, with triangles
    229.     /// that are intersected by the partition plane being placed in both sets
    230.     /// </summary>
    231.     void Split(List<int> triangles, Vector3 partitionPoint, Vector3 partitionNormal, out List<int> positiveTriangles, out List<int> negativeTriangles)
    232.     {
    233.         positiveTriangles = new List<int>();
    234.         negativeTriangles = new List<int>();
    235.  
    236.         foreach (int triangle in triangles)
    237.         {
    238.             bool firstPointAbove = PointAbovePlane(partitionPoint, partitionNormal, vertices[tris[triangle]]);
    239.             bool secondPointAbove = PointAbovePlane(partitionPoint, partitionNormal, vertices[tris[triangle + 1]]);
    240.             bool thirdPointAbove = PointAbovePlane(partitionPoint, partitionNormal, vertices[tris[triangle + 2]]);
    241.  
    242.             if (firstPointAbove && secondPointAbove && thirdPointAbove)
    243.             {
    244.                 positiveTriangles.Add(triangle);
    245.             }
    246.             else if (!firstPointAbove && !secondPointAbove && !thirdPointAbove)
    247.             {
    248.                 negativeTriangles.Add(triangle);
    249.             }
    250.             else
    251.             {
    252.                 positiveTriangles.Add(triangle);
    253.                 negativeTriangles.Add(triangle);
    254.             }
    255.         }
    256.     }
    257.  
    258.     bool PointAbovePlane(Vector3 planeOrigin, Vector3 planeNormal, Vector3 point)
    259.     {
    260.         return Vector3.Dot(point - planeOrigin, planeNormal) >= 0;
    261.     }
    262.  
    263.     float PointDistanceFromPlane(Vector3 planeOrigin, Vector3 planeNormal, Vector3 point)
    264.     {
    265.         return Mathf.Abs(Vector3.Dot((point - planeOrigin), planeNormal));
    266.     }
    267.  
    268.     /// <summary>
    269.     /// Determines the closest point between a point and a triangle.
    270.     /// Borrowed from RPGMesh class of the RPGController package for Unity, by fholm
    271.     /// The code in this method is copyrighted by the SlimDX Group under the MIT license:
    272.     ///
    273.     /// Copyright (c) 2007-2010 SlimDX Group
    274.     ///
    275.     /// Permission is hereby granted, free of charge, to any person obtaining a copy
    276.     /// of this software and associated documentation files (the "Software"), to deal
    277.     /// in the Software without restriction, including without limitation the rights
    278.     /// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    279.     /// copies of the Software, and to permit persons to whom the Software is
    280.     /// furnished to do so, subject to the following conditions:
    281.     ///
    282.     /// The above copyright notice and this permission notice shall be included in
    283.     /// all copies or substantial portions of the Software.
    284.     ///
    285.     /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    286.     /// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    287.     /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    288.     /// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    289.     /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    290.     /// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    291.     /// THE SOFTWARE.
    292.     ///
    293.     /// </summary>
    294.     /// <param name="point">The point to test.</param>
    295.     /// <param name="vertex1">The first vertex to test.</param>
    296.     /// <param name="vertex2">The second vertex to test.</param>
    297.     /// <param name="vertex3">The third vertex to test.</param>
    298.     /// <param name="result">When the method completes, contains the closest point between the two objects.</param>
    299.     public static void ClosestPointOnTriangleToPoint(ref Vector3 vertex1, ref Vector3 vertex2, ref Vector3 vertex3, ref Vector3 point, out Vector3 result)
    300.     {
    301.         //Source: Real-Time Collision Detection by Christer Ericson
    302.         //Reference: Page 136
    303.  
    304.         //Check if P in vertex region outside A
    305.         Vector3 ab = vertex2 - vertex1;
    306.         Vector3 ac = vertex3 - vertex1;
    307.         Vector3 ap = point - vertex1;
    308.  
    309.         float d1 = Vector3.Dot(ab, ap);
    310.         float d2 = Vector3.Dot(ac, ap);
    311.         if (d1 <= 0.0f && d2 <= 0.0f)
    312.         {
    313.             result = vertex1; //Barycentric coordinates (1,0,0)
    314.             return;
    315.         }
    316.  
    317.         //Check if P in vertex region outside B
    318.         Vector3 bp = point - vertex2;
    319.         float d3 = Vector3.Dot(ab, bp);
    320.         float d4 = Vector3.Dot(ac, bp);
    321.         if (d3 >= 0.0f && d4 <= d3)
    322.         {
    323.             result = vertex2; // barycentric coordinates (0,1,0)
    324.             return;
    325.         }
    326.  
    327.         //Check if P in edge region of AB, if so return projection of P onto AB
    328.         float vc = d1 * d4 - d3 * d2;
    329.         if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
    330.         {
    331.             float v = d1 / (d1 - d3);
    332.             result = vertex1 + v * ab; //Barycentric coordinates (1-v,v,0)
    333.             return;
    334.         }
    335.  
    336.         //Check if P in vertex region outside C
    337.         Vector3 cp = point - vertex3;
    338.         float d5 = Vector3.Dot(ab, cp);
    339.         float d6 = Vector3.Dot(ac, cp);
    340.         if (d6 >= 0.0f && d5 <= d6)
    341.         {
    342.             result = vertex3; //Barycentric coordinates (0,0,1)
    343.             return;
    344.         }
    345.  
    346.         //Check if P in edge region of AC, if so return projection of P onto AC
    347.         float vb = d5 * d2 - d1 * d6;
    348.         if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
    349.         {
    350.             float w = d2 / (d2 - d6);
    351.             result = vertex1 + w * ac; //Barycentric coordinates (1-w,0,w)
    352.             return;
    353.         }
    354.  
    355.         //Check if P in edge region of BC, if so return projection of P onto BC
    356.         float va = d3 * d6 - d5 * d4;
    357.         if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
    358.         {
    359.             float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
    360.             result = vertex2 + w * (vertex3 - vertex2); //Barycentric coordinates (0,1-w,w)
    361.             return;
    362.         }
    363.  
    364.         //P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    365.         float denom = 1.0f / (va + vb + vc);
    366.         float v2 = vb * denom;
    367.         float w2 = vc * denom;
    368.         result = vertex1 + ab * v2 + ac * w2; //= u*vertex1 + v*vertex2 + w*vertex3, u = va * denom = 1.0f - v - w
    369.     }
    370.  
    371.     void DrawTriangleSet(int[] triangles, Color color)
    372.     {
    373.         /*foreach (int triangle in triangles)
    374.         {
    375.             Debug.DrawTriangle(vertices[tris[triangle]], vertices[tris[triangle + 1]], vertices[tris[triangle + 2]], color, transform);
    376.         }*/
    377.     }
    378. }
    Code (CSharp):
    1. using UnityEngine;
    2. using System.Collections;
    3.  
    4. public static class SuperCollider
    5. {
    6.  
    7.     public static Vector3 ClosestPointOnSurface(Collider collider, Vector3 to, float radius)
    8.     {
    9.         if (collider is BoxCollider)
    10.         {
    11.             return SuperCollider.ClosestPointOnSurface((BoxCollider)collider, to);
    12.         }
    13.         else if (collider is SphereCollider)
    14.         {
    15.             return SuperCollider.ClosestPointOnSurface((SphereCollider)collider, to);
    16.         }
    17.         else if (collider is CapsuleCollider)
    18.         {
    19.             return SuperCollider.ClosestPointOnSurface((CapsuleCollider)collider, to);
    20.         }
    21.         else if (collider is MeshCollider)
    22.         {
    23.             /*RPGMesh rpgMesh = collider.GetComponent<RPGMesh>();
    24.  
    25.             if (rpgMesh != null)
    26.             {
    27.                 return rpgMesh.ClosestPointOn(to, radius, false, false);
    28.             }*/
    29.  
    30.             BSPTree bsp = collider.GetComponent<BSPTree>();
    31.  
    32.             if (bsp != null)
    33.             {
    34.                 return bsp.ClosestPointOn(to, radius);
    35.             }
    36.  
    37.             /*BruteForceMesh bfm = collider.GetComponent<BruteForceMesh>();
    38.  
    39.             if (bfm != null)
    40.             {
    41.                 return bfm.ClosestPointOn(to);
    42.             }*/
    43.         }
    44.         else if (collider is TerrainCollider)
    45.         {
    46.             return SuperCollider.ClosestPointOnSurface((TerrainCollider)collider, to, radius, false);
    47.         }
    48.  
    49.         return Vector3.zero;
    50.     }
    51.  
    52.     public static Vector3 ClosestPointOnSurface(SphereCollider collider, Vector3 to)
    53.     {
    54.         Vector3 p;
    55.  
    56.         p = to - collider.transform.position;
    57.         p.Normalize();
    58.  
    59.         p *= collider.radius * collider.transform.localScale.x;
    60.         p += collider.transform.position;
    61.  
    62.         return p;
    63.     }
    64.  
    65.     public static Vector3 ClosestPointOnSurface(BoxCollider collider, Vector3 to)
    66.     {
    67.         // Cache the collider transform
    68.         var ct = collider.transform;
    69.  
    70.         // Firstly, transform the point into the space of the collider
    71.         var local = ct.InverseTransformPoint(to);
    72.  
    73.         // Now, shift it to be in the center of the box
    74.         local -= collider.center;
    75.  
    76.         //Pre multiply to save operations.
    77.         var halfSize = collider.size * 0.5f;
    78.  
    79.         // Clamp the points to the collider's extents
    80.         var localNorm = new Vector3(
    81.                 Mathf.Clamp(local.x, -halfSize.x, halfSize.x),
    82.                 Mathf.Clamp(local.y, -halfSize.y, halfSize.y),
    83.                 Mathf.Clamp(local.z, -halfSize.z, halfSize.z)
    84.             );
    85.  
    86.         //Calculate distances from each edge
    87.         var dx = Mathf.Min(Mathf.Abs(halfSize.x - localNorm.x), Mathf.Abs(-halfSize.x - localNorm.x));
    88.         var dy = Mathf.Min(Mathf.Abs(halfSize.y - localNorm.y), Mathf.Abs(-halfSize.y - localNorm.y));
    89.         var dz = Mathf.Min(Mathf.Abs(halfSize.z - localNorm.z), Mathf.Abs(-halfSize.z - localNorm.z));
    90.  
    91.         // Select a face to project on
    92.         if (dx < dy && dx < dz)
    93.         {
    94.             localNorm.x = Mathf.Sign(localNorm.x) * halfSize.x;
    95.         }
    96.         else if (dy < dx && dy < dz)
    97.         {
    98.             localNorm.y = Mathf.Sign(localNorm.y) * halfSize.y;
    99.         }
    100.         else if (dz < dx && dz < dy)
    101.         {
    102.             localNorm.z = Mathf.Sign(localNorm.z) * halfSize.z;
    103.         }
    104.  
    105.         // Now we undo our transformations
    106.         localNorm += collider.center;
    107.  
    108.         // Return resulting point
    109.         return ct.TransformPoint(localNorm);
    110.     }
    111.  
    112.     // Courtesy of Moodie
    113.     public static Vector3 ClosestPointOnSurface(CapsuleCollider collider, Vector3 to)
    114.     {
    115.         Transform ct = collider.transform; // Transform of the collider
    116.  
    117.         float lineLength = collider.height - collider.radius * 2; // The length of the line connecting the center of both sphere
    118.         Vector3 dir = Vector3.up;
    119.  
    120.         Vector3 upperSphere = dir * lineLength * 0.5f + collider.center; // The position of the radius of the upper sphere in local coordinates
    121.         Vector3 lowerSphere = -dir * lineLength * 0.5f + collider.center; // The position of the radius of the lower sphere in local coordinates
    122.  
    123.         Vector3 local = ct.InverseTransformPoint(to); // The position of the controller in local coordinates
    124.  
    125.         Vector3 p = Vector3.zero; // Contact point
    126.         Vector3 pt = Vector3.zero; // The point we need to use to get a direction vector with the controller to calculate contact point
    127.  
    128.         if (local.y < lineLength * 0.5f && local.y > -lineLength * 0.5f) // Controller is contacting with cylinder, not spheres
    129.             pt = dir * local.y + collider.center;
    130.         else if (local.y > lineLength * 0.5f) // Controller is contacting with the upper sphere
    131.             pt = upperSphere;
    132.         else if (local.y < -lineLength * 0.5f) // Controller is contacting with lower sphere
    133.             pt = lowerSphere;
    134.  
    135.         //Calculate contact point in local coordinates and return it in world coordinates
    136.         p = local - pt;
    137.         p.Normalize();
    138.         p = p * collider.radius + pt;
    139.         return ct.TransformPoint(p);
    140.  
    141.     }
    142.  
    143.     public static Vector3 ClosestPointOnSurface(TerrainCollider collider, Vector3 to, float radius, bool debug = false)
    144.     {
    145.         var terrainData = collider.terrainData;
    146.  
    147.         var local = collider.transform.InverseTransformPoint(to);
    148.  
    149.         // Calculate the size of each tile on the terrain horizontally and vertically
    150.         float pixelSizeX = terrainData.size.x / (terrainData.heightmapResolution - 1);
    151.         float pixelSizeZ = terrainData.size.z / (terrainData.heightmapResolution - 1);
    152.  
    153.         var percentZ = Mathf.Clamp01(local.z / terrainData.size.z);
    154.         var percentX = Mathf.Clamp01(local.x / terrainData.size.x);
    155.  
    156.         float positionX = percentX * (terrainData.heightmapResolution - 1);
    157.         float positionZ = percentZ * (terrainData.heightmapResolution - 1);
    158.  
    159.         // Calculate our position, in tiles, on the terrain
    160.         int pixelX = Mathf.FloorToInt(positionX);
    161.         int pixelZ = Mathf.FloorToInt(positionZ);
    162.  
    163.         // Calculate the distance from our point to the edge of the tile we are in
    164.         float distanceX = (positionX - pixelX) * pixelSizeX;
    165.         float distanceZ = (positionZ - pixelZ) * pixelSizeZ;
    166.  
    167.         // Find out how many tiles we are overlapping on the X plane
    168.         float radiusExtentsLeftX = radius - distanceX;
    169.         float radiusExtentsRightX = radius - (pixelSizeX - distanceX);
    170.  
    171.         int overlappedTilesXLeft = radiusExtentsLeftX > 0 ? Mathf.FloorToInt(radiusExtentsLeftX / pixelSizeX) + 1 : 0;
    172.         int overlappedTilesXRight = radiusExtentsRightX > 0 ? Mathf.FloorToInt(radiusExtentsRightX / pixelSizeX) + 1 : 0;
    173.  
    174.         // Find out how many tiles we are overlapping on the Z plane
    175.         float radiusExtentsLeftZ = radius - distanceZ;
    176.         float radiusExtentsRightZ = radius - (pixelSizeZ - distanceZ);
    177.  
    178.         int overlappedTilesZLeft = radiusExtentsLeftZ > 0 ? Mathf.FloorToInt(radiusExtentsLeftZ / pixelSizeZ) + 1 : 0;
    179.         int overlappedTilesZRight = radiusExtentsRightZ > 0 ? Mathf.FloorToInt(radiusExtentsRightZ / pixelSizeZ) + 1 : 0;
    180.  
    181.         // Retrieve the heights of the pixels we are testing against
    182.         int startPositionX = pixelX - overlappedTilesXLeft;
    183.         int startPositionZ = pixelZ - overlappedTilesZLeft;
    184.  
    185.         int numberOfXPixels = overlappedTilesXRight + overlappedTilesXLeft + 1;
    186.         int numberOfZPixels = overlappedTilesZRight + overlappedTilesZLeft + 1;
    187.  
    188.         // Account for if we are off the terrain
    189.         if (startPositionX < 0)
    190.         {
    191.             numberOfXPixels -= Mathf.Abs(startPositionX);
    192.             startPositionX = 0;
    193.         }
    194.  
    195.         if (startPositionZ < 0)
    196.         {
    197.             numberOfZPixels -= Mathf.Abs(startPositionZ);
    198.             startPositionZ = 0;
    199.         }
    200.  
    201.         if (startPositionX + numberOfXPixels + 1 > terrainData.heightmapResolution)
    202.         {
    203.             numberOfXPixels = terrainData.heightmapResolution - startPositionX - 1;
    204.         }
    205.  
    206.         if (startPositionZ + numberOfZPixels + 1 > terrainData.heightmapResolution)
    207.         {
    208.             numberOfZPixels = terrainData.heightmapResolution - startPositionZ - 1;
    209.         }
    210.  
    211.         // Retrieve the heights of the tile we are in and all overlapped tiles
    212.         var heights = terrainData.GetHeights(startPositionX, startPositionZ, numberOfXPixels + 1, numberOfZPixels + 1);
    213.  
    214.         // Pre-scale the heights data to be world-scale instead of 0...1
    215.         for (int i = 0; i < numberOfXPixels + 1; i++)
    216.         {
    217.             for (int j = 0; j < numberOfZPixels + 1; j++)
    218.             {
    219.                 heights[j, i] *= terrainData.size.y;
    220.             }
    221.         }
    222.  
    223.         // Find the shortest distance to any triangle in the set gathered
    224.         float shortestDistance = float.MaxValue;
    225.  
    226.         Vector3 shortestPoint = Vector3.zero;
    227.  
    228.         for (int x = 0; x < numberOfXPixels; x++)
    229.         {
    230.             for (int z = 0; z < numberOfZPixels; z++)
    231.             {
    232.                 // Build the set of points that creates the two triangles that form this tile
    233.                 Vector3 a = new Vector3((startPositionX + x) * pixelSizeX, heights[z, x], (startPositionZ + z) * pixelSizeZ);
    234.                 Vector3 b = new Vector3((startPositionX + x + 1) * pixelSizeX, heights[z, x + 1], (startPositionZ + z) * pixelSizeZ);
    235.                 Vector3 c = new Vector3((startPositionX + x) * pixelSizeX, heights[z + 1, x], (startPositionZ + z + 1) * pixelSizeZ);
    236.                 Vector3 d = new Vector3((startPositionX + x + 1) * pixelSizeX, heights[z + 1, x + 1], (startPositionZ + z + 1) * pixelSizeZ);
    237.  
    238.                 Vector3 nearest;
    239.  
    240.                 BSPTree.ClosestPointOnTriangleToPoint(ref a, ref d, ref c, ref local, out nearest);
    241.  
    242.                 float distance = (local - nearest).sqrMagnitude;
    243.  
    244.                 if (distance <= shortestDistance)
    245.                 {
    246.                     shortestDistance = distance;
    247.                     shortestPoint = nearest;
    248.                 }
    249.  
    250.                 BSPTree.ClosestPointOnTriangleToPoint(ref a, ref b, ref d, ref local, out nearest);
    251.  
    252.                 distance = (local - nearest).sqrMagnitude;
    253.  
    254.                 if (distance <= shortestDistance)
    255.                 {
    256.                     shortestDistance = distance;
    257.                     shortestPoint = nearest;
    258.                 }
    259.  
    260.                 /*if (debug)
    261.                 {
    262.                     DebugDraw.DrawTriangle(a, d, c, Color.cyan);
    263.                     DebugDraw.DrawTriangle(a, b, d, Color.red);
    264.                 }*/
    265.             }
    266.         }
    267.  
    268.         return collider.transform.TransformPoint(shortestPoint);
    269.     }
    270. }
    And i'm not even having a big map.
     
  9. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    I didnt use your code above, but I took from the github and got rid of some garbage causing things and did some profiling. I also disabled the meshrenderers and got rid of lights so that they wouldnt mess up the test.

    Im thinking the BSPTree might not be doing to well. If I use a regular unity cube with a mesh collider, then with 100 of those cubes spawned I get about 100 frames while doing calculations where I get about 150 frames where there is no calculations being done, which seems fine. However, if I use 100 of the default blender monkey mesh, which I think imported into unity it has like 950 or so triangles, I get like 5 fps if my sphere is overlapping a lot of triangles.
    The thing is, even if I make my sphere small and place it in between one triangle, the bsp is still doing the calculations on like 4 triangles. I am not sure if thats considered good or bad as I never made a bsptree. The framerate is a lot better though with only 4 triangles.
    Edit - Its not necessarily the amount of triangles that we calculate on that causes the lag, but how much work the bsptree needs to do to get those triangles.

    This might not be to much of a concern for me since I am just using this for a single player and its not like I am going to be intersecting over 100 meshes or a ton of triangles, but I guess maybe in your case it might be different.

    How did you have things setup? Just to be sure, you werent doing a bunch of Debug.Logs right? Those cause a bunch of lag.
    Were you using a terrain collider? I never used / tested on those.
     
    Last edited: Apr 5, 2016
  10. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    No, I was using a Mesh Collider, but I can't afford that kind of script, because I have to do it multiple times per frame, so it doesn't helps at all.
     
  11. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    I am using that script many times per frame (although on simple meshes). I am just curious as to what your setup was to get you such poor performance. Is your sphere over a bunch of triangles? When you say multiple times a frame, do you mean 20, or 1000?

    Unfortunately from all my searching, this script is the best I could find, unless if you want to integrate a full physics engine maybe? Like how this guy tried and made opensource
    http://forum.unity3d.com/threads/jitter-physics-engine-vs-built-in-physx.186325/#post-2518244
     
  12. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    6 times per frame, over a map of 150k tris.
    I have an i5 4440S.
     
  13. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    I guess a single mesh of 150k triangles is to much for this BSPTree. Maybe you can look into a better space partitioning method to help boost performance as I dont think its the actual ClosestPointOnTriangle method that is slow, but the bsptree trying to find the least amount of triangles to work on.
    Otherwise, I wouldnt put your hopes up too much on finding an alternative =\. I spent a good bit looking, but I guess you never know =)

    Another option might be to use a rigidbody with its movement and rotation frozen in the inspector (could be set in code too) and use OnCollisionEnter/Stay/Exit to get the contact points. The problem with rigidbody stuff is its 1 frame delay though.
    Also, oncollisionxxx causes garbage collection. Im also not sure if the rigidbody needs to move to detect the collisions.
     
    Last edited: Apr 3, 2016
  14. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    I'm actually using 8 raycast around a point when my SphereCast fail, and it works well.
     
    HiddenMonk likes this.
  15. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    Edit - The reason the KDTree seems to perform better than the bsptree is because the kdtree is only searching for the closest vertex and not all the triangles that are touching the sphere. This means the kdtree is not accurate depending on the mesh. Also, due to the nature of the bsptree splitting between triangles, there are a lot of duplicate triangles being checked over. By using a hashset instead of a list we can try to help the issue, but it isnt perfect. The bsp tree also seems to sometimes include triangles that are way out of range of the sphere, so doing a boundingbox check of the triangles can also help with that.

    ------------------------------------------------------------

    I remembered in the past when searching for a way to get the closest point to a mesh, I ran into this thread
    http://forum.unity3d.com/threads/closest-point-on-mesh-collider.34660/#post-225658
    Edit - also just found this
    http://forum.unity3d.com/threads/point-nearest-neighbour-search-class.29923/

    That post by andeeee had a method for getting the closest point with a KDTree, but I think at the time I had some issues with it.
    Going back to it now, the problem seems to be that the KDTree finds a vertex that is closest, but doesnt take into account that there can be vertices that are in the exact same spot. Unity takes meshes and duplicates the vertices in order to have the normals be calculated properly.
    I have added a fix to the KDTree with the method FindNearestEpsilon which will find all the nearest that are within epsilon range of eachother.

    I was curious and wanted to see if I could use his KDTree instead of the BSPTree used here to increase performance, and boy did it increase performance! (at least from the few tests I ran).

    I would like you to test out the code to see if its also a big improvement on your side.
    Here is the code
    Code (CSharp):
    1.  
    2. using UnityEngine;
    3. using System.Collections;
    4.  
    5. public static class SuperCollider {
    6.  
    7.     public static Vector3 ClosestPointOnSurface(Collider collider, Vector3 to, float radius)
    8.     {
    9.         if (collider is BoxCollider)
    10.         {
    11.             return SuperCollider.ClosestPointOnSurface((BoxCollider)collider, to);
    12.         }
    13.         else if (collider is SphereCollider)
    14.         {
    15.             return SuperCollider.ClosestPointOnSurface((SphereCollider)collider, to);
    16.         }
    17.         else if (collider is CapsuleCollider)
    18.         {
    19.             return SuperCollider.ClosestPointOnSurface((CapsuleCollider)collider, to);
    20.         }
    21.         else if (collider is MeshCollider)
    22.         {
    23.             MeshKDTree meshKDTree = collider.GetComponent<MeshKDTree>();
    24.  
    25.             if (meshKDTree != null)
    26.             {
    27.                 return meshKDTree.ClosestPointOnSurface(to);
    28.             }
    29.         }
    30.         else if (collider is TerrainCollider)
    31.         {
    32.             return SuperCollider.ClosestPointOnSurface((TerrainCollider)collider, to, radius, false);
    33.         }
    34.  
    35.         return Vector3.zero;
    36.     }
    37.  
    38.     public static Vector3 ClosestPointOnSurface(SphereCollider collider, Vector3 to)
    39.     {
    40.         Vector3 p;
    41.  
    42.         p = to - collider.transform.position;
    43.         p.Normalize();
    44.  
    45.         p *= collider.radius * collider.transform.localScale.x;
    46.         p += collider.transform.position;
    47.  
    48.         return p;
    49.     }
    50.  
    51.     public static Vector3 ClosestPointOnSurface(BoxCollider collider, Vector3 to)
    52.     {
    53.         // Cache the collider transform
    54.         var ct = collider.transform;
    55.  
    56.         // Firstly, transform the point into the space of the collider
    57.         var local = ct.InverseTransformPoint(to);
    58.  
    59.         // Now, shift it to be in the center of the box
    60.         local -= collider.center;
    61.  
    62.         //Pre multiply to save operations.
    63.         var halfSize = collider.size * 0.5f;
    64.  
    65.         // Clamp the points to the collider's extents
    66.         var localNorm = new Vector3(
    67.                 Mathf.Clamp(local.x, -halfSize.x, halfSize.x),
    68.                 Mathf.Clamp(local.y, -halfSize.y, halfSize.y),
    69.                 Mathf.Clamp(local.z, -halfSize.z, halfSize.z)
    70.             );
    71.  
    72.         //Calculate distances from each edge
    73.         var dx = Mathf.Min(Mathf.Abs(halfSize.x - localNorm.x), Mathf.Abs(-halfSize.x - localNorm.x));
    74.         var dy = Mathf.Min(Mathf.Abs(halfSize.y - localNorm.y), Mathf.Abs(-halfSize.y - localNorm.y));
    75.         var dz = Mathf.Min(Mathf.Abs(halfSize.z - localNorm.z), Mathf.Abs(-halfSize.z - localNorm.z));
    76.  
    77.         // Select a face to project on
    78.         if (dx < dy && dx < dz)
    79.         {
    80.             localNorm.x = Mathf.Sign(localNorm.x) * halfSize.x;
    81.         }
    82.         else if (dy < dx && dy < dz)
    83.         {
    84.             localNorm.y = Mathf.Sign(localNorm.y) * halfSize.y;
    85.         }
    86.         else if (dz < dx && dz < dy)
    87.         {
    88.             localNorm.z = Mathf.Sign(localNorm.z) * halfSize.z;
    89.         }
    90.  
    91.         // Now we undo our transformations
    92.         localNorm += collider.center;
    93.  
    94.         // Return resulting point
    95.         return ct.TransformPoint(localNorm);
    96.     }
    97.  
    98.     // Courtesy of Moodie
    99.     public static Vector3 ClosestPointOnSurface(CapsuleCollider collider, Vector3 to)
    100.     {
    101.         Transform ct = collider.transform; // Transform of the collider
    102.  
    103.         float lineLength = collider.height - collider.radius * 2; // The length of the line connecting the center of both sphere
    104.         Vector3 dir = Vector3.up;
    105.  
    106.         Vector3 upperSphere = dir * lineLength * 0.5f + collider.center; // The position of the radius of the upper sphere in local coordinates
    107.         Vector3 lowerSphere = -dir * lineLength * 0.5f + collider.center; // The position of the radius of the lower sphere in local coordinates
    108.  
    109.         Vector3 local = ct.InverseTransformPoint(to); // The position of the controller in local coordinates
    110.  
    111.         Vector3 p = Vector3.zero; // Contact point
    112.         Vector3 pt = Vector3.zero; // The point we need to use to get a direction vector with the controller to calculate contact point
    113.  
    114.         if (local.y < lineLength * 0.5f && local.y > -lineLength * 0.5f) // Controller is contacting with cylinder, not spheres
    115.             pt = dir * local.y + collider.center;
    116.         else if (local.y > lineLength * 0.5f) // Controller is contacting with the upper sphere
    117.             pt = upperSphere;
    118.         else if (local.y < -lineLength * 0.5f) // Controller is contacting with lower sphere
    119.             pt = lowerSphere;
    120.  
    121.         //Calculate contact point in local coordinates and return it in world coordinates
    122.         p = local - pt;
    123.         p.Normalize();
    124.         p = p * collider.radius + pt;
    125.         return ct.TransformPoint(p);
    126.  
    127.     }
    128.  
    129.     public static Vector3 ClosestPointOnSurface(TerrainCollider collider, Vector3 to, float radius, bool debug=false)
    130.     {
    131.         var terrainData = collider.terrainData;
    132.  
    133.         var local = collider.transform.InverseTransformPoint(to);
    134.  
    135.         // Calculate the size of each tile on the terrain horizontally and vertically
    136.         float pixelSizeX = terrainData.size.x / (terrainData.heightmapResolution - 1);
    137.         float pixelSizeZ = terrainData.size.z / (terrainData.heightmapResolution - 1);
    138.  
    139.         var percentZ = Mathf.Clamp01(local.z / terrainData.size.z);
    140.         var percentX = Mathf.Clamp01(local.x / terrainData.size.x);
    141.  
    142.         float positionX = percentX * (terrainData.heightmapResolution - 1);
    143.         float positionZ = percentZ * (terrainData.heightmapResolution - 1);
    144.  
    145.         // Calculate our position, in tiles, on the terrain
    146.         int pixelX = Mathf.FloorToInt(positionX);
    147.         int pixelZ = Mathf.FloorToInt(positionZ);
    148.  
    149.         // Calculate the distance from our point to the edge of the tile we are in
    150.         float distanceX = (positionX - pixelX) * pixelSizeX;
    151.         float distanceZ = (positionZ - pixelZ) * pixelSizeZ;
    152.  
    153.         // Find out how many tiles we are overlapping on the X plane
    154.         float radiusExtentsLeftX = radius - distanceX;
    155.         float radiusExtentsRightX = radius - (pixelSizeX - distanceX);
    156.  
    157.         int overlappedTilesXLeft = radiusExtentsLeftX > 0 ? Mathf.FloorToInt(radiusExtentsLeftX / pixelSizeX) + 1 : 0;
    158.         int overlappedTilesXRight = radiusExtentsRightX > 0 ? Mathf.FloorToInt(radiusExtentsRightX / pixelSizeX) + 1 : 0;
    159.  
    160.         // Find out how many tiles we are overlapping on the Z plane
    161.         float radiusExtentsLeftZ = radius - distanceZ;
    162.         float radiusExtentsRightZ = radius - (pixelSizeZ - distanceZ);
    163.  
    164.         int overlappedTilesZLeft = radiusExtentsLeftZ > 0 ? Mathf.FloorToInt(radiusExtentsLeftZ / pixelSizeZ) + 1 : 0;
    165.         int overlappedTilesZRight = radiusExtentsRightZ > 0 ? Mathf.FloorToInt(radiusExtentsRightZ / pixelSizeZ) + 1 : 0;
    166.  
    167.         // Retrieve the heights of the pixels we are testing against
    168.         int startPositionX = pixelX - overlappedTilesXLeft;
    169.         int startPositionZ = pixelZ - overlappedTilesZLeft;
    170.  
    171.         int numberOfXPixels = overlappedTilesXRight + overlappedTilesXLeft + 1;
    172.         int numberOfZPixels = overlappedTilesZRight + overlappedTilesZLeft + 1;
    173.  
    174.         // Account for if we are off the terrain
    175.         if (startPositionX < 0)
    176.         {
    177.             numberOfXPixels -= Mathf.Abs(startPositionX);
    178.             startPositionX = 0;
    179.         }
    180.  
    181.         if (startPositionZ < 0)
    182.         {
    183.             numberOfZPixels -= Mathf.Abs(startPositionZ);
    184.             startPositionZ = 0;
    185.         }
    186.  
    187.         if (startPositionX + numberOfXPixels + 1 > terrainData.heightmapResolution)
    188.         {
    189.             numberOfXPixels = terrainData.heightmapResolution - startPositionX - 1;
    190.         }
    191.  
    192.         if (startPositionZ + numberOfZPixels + 1 > terrainData.heightmapResolution)
    193.         {
    194.             numberOfZPixels = terrainData.heightmapResolution - startPositionZ - 1;
    195.         }
    196.  
    197.         // Retrieve the heights of the tile we are in and all overlapped tiles
    198.         var heights = terrainData.GetHeights(startPositionX, startPositionZ, numberOfXPixels + 1, numberOfZPixels + 1);
    199.  
    200.         // Pre-scale the heights data to be world-scale instead of 0...1
    201.         for (int i = 0; i < numberOfXPixels + 1; i++)
    202.         {
    203.             for (int j = 0; j < numberOfZPixels + 1; j++)
    204.             {
    205.                 heights[j, i] *= terrainData.size.y;
    206.             }
    207.         }
    208.  
    209.         // Find the shortest distance to any triangle in the set gathered
    210.         float shortestDistance = float.MaxValue;
    211.  
    212.         Vector3 shortestPoint = Vector3.zero;
    213.  
    214.         for (int x = 0; x < numberOfXPixels; x++)
    215.         {
    216.             for (int z = 0; z < numberOfZPixels; z++)
    217.             {
    218.                 // Build the set of points that creates the two triangles that form this tile
    219.                 Vector3 a = new Vector3((startPositionX + x) * pixelSizeX, heights[z, x], (startPositionZ + z) * pixelSizeZ);
    220.                 Vector3 b = new Vector3((startPositionX + x + 1) * pixelSizeX, heights[z, x + 1], (startPositionZ + z) * pixelSizeZ);
    221.                 Vector3 c = new Vector3((startPositionX + x) * pixelSizeX, heights[z + 1, x], (startPositionZ + z + 1) * pixelSizeZ);
    222.                 Vector3 d = new Vector3((startPositionX + x + 1) * pixelSizeX, heights[z + 1, x + 1], (startPositionZ + z + 1) * pixelSizeZ);
    223.  
    224.                 Vector3 nearest;
    225.  
    226.                 BSPTree.ClosestPointOnTriangleToPoint(ref a, ref d, ref c, ref local, out nearest);
    227.  
    228.                 float distance = (local - nearest).sqrMagnitude;
    229.  
    230.                 if (distance <= shortestDistance)
    231.                 {
    232.                     shortestDistance = distance;
    233.                     shortestPoint = nearest;
    234.                 }
    235.  
    236.                 BSPTree.ClosestPointOnTriangleToPoint(ref a, ref b, ref d, ref local, out nearest);
    237.  
    238.                 distance = (local - nearest).sqrMagnitude;
    239.  
    240.                 if (distance <= shortestDistance)
    241.                 {
    242.                     shortestDistance = distance;
    243.                     shortestPoint = nearest;
    244.                 }
    245.  
    246.                 if (debug)
    247.                 {
    248.                     //DebugDraw.DrawTriangle(a, d, c, Color.cyan);
    249.                     //DebugDraw.DrawTriangle(a, b, d, Color.red);
    250.                 }
    251.             }
    252.         }
    253.  
    254.         return collider.transform.TransformPoint(shortestPoint);
    255.     }
    256. }
    Code (CSharp):
    1.  
    2. using UnityEngine;
    3. using System.Collections.Generic;
    4.  
    5. [RequireComponent(typeof(MeshCollider))]
    6. public class MeshKDTree : MonoBehaviour
    7. {
    8.     int[] tris;
    9.     Vector3[] verts;
    10.     KDTree kd;
    11.     VertTriList vt;
    12.  
    13.     void Awake()
    14.     {
    15.         MeshFilter mf = GetComponent<MeshFilter>();
    16.         Mesh mesh = mf.mesh;
    17.         vt = new VertTriList(mesh);
    18.         verts = mesh.vertices;
    19.         tris = mesh.triangles;
    20.         kd = KDTree.MakeFromPoints(verts);
    21.     }
    22.  
    23.     public Vector3 ClosestPointOnSurface(Vector3 position)
    24.     {
    25.         position = transform.InverseTransformPoint(position);
    26.         return transform.TransformPoint(NearestPointOnMesh(position, verts, kd, tris, vt));
    27.     }
    28.  
    29.     List<int> nearests = new List<int>();
    30.     Vector3 NearestPointOnMesh(Vector3 pt, Vector3[] verts, KDTree vertProx, int[] tri, VertTriList vt)
    31.     {
    32.         //    First, find the nearest vertex (the nearest point must be on one of the triangles
    33.         //    that uses this vertex if the mesh is convex).
    34.         //  Since there can be multiple vertices on a single spot, we need to find the correct vert and triangle.
    35.         vertProx.FindNearestEpsilon(pt, nearests);
    36.  
    37.         Vector3 nearestPt = Vector3.zero;
    38.         float nearestSqDist = 100000000f;
    39.         Vector3 possNearestPt;
    40.  
    41.         for(int i = 0; i < nearests.Count; i++)
    42.         {
    43.             //    Get the list of triangles in which the nearest vert "participates".
    44.             int[] nearTris = vt[nearests[i]];
    45.  
    46.             for(int j = 0; j < nearTris.Length; j++)
    47.             {
    48.                 int triOff = nearTris[j] * 3;
    49.                 Vector3 a = verts[tri[triOff]];
    50.                 Vector3 b = verts[tri[triOff + 1]];
    51.                 Vector3 c = verts[tri[triOff + 2]];
    52.  
    53.                 ClosestPointOnTriangleToPoint(ref pt, ref a, ref b, ref c, out possNearestPt);
    54.                 float possNearestSqDist = (pt - possNearestPt).sqrMagnitude;
    55.  
    56.                 if(possNearestSqDist < nearestSqDist)
    57.                 {
    58.                     nearestPt = possNearestPt;
    59.                     nearestSqDist = possNearestSqDist;
    60.                 }
    61.             }
    62.         }
    63.  
    64.         return nearestPt;
    65.     }
    66.  
    67.     public static void ClosestPointOnTriangleToPoint(ref Vector3 point, ref Vector3 vertex1, ref Vector3 vertex2, ref Vector3 vertex3, out Vector3 result)
    68.     {
    69.         //Source: Real-Time Collision Detection by Christer Ericson
    70.         //Reference: Page 136
    71.  
    72.         //Check if P in vertex region outside A
    73.         Vector3 ab = vertex2 - vertex1;
    74.         Vector3 ac = vertex3 - vertex1;
    75.         Vector3 ap = point - vertex1;
    76.  
    77.         float d1 = Vector3.Dot(ab, ap);
    78.         float d2 = Vector3.Dot(ac, ap);
    79.         if (d1 <= 0.0f && d2 <= 0.0f)
    80.         {
    81.             result = vertex1; //Barycentric coordinates (1,0,0)
    82.             return;
    83.         }
    84.  
    85.         //Check if P in vertex region outside B
    86.         Vector3 bp = point - vertex2;
    87.         float d3 = Vector3.Dot(ab, bp);
    88.         float d4 = Vector3.Dot(ac, bp);
    89.         if (d3 >= 0.0f && d4 <= d3)
    90.         {
    91.             result = vertex2; // barycentric coordinates (0,1,0)
    92.             return;
    93.         }
    94.  
    95.         //Check if P in edge region of AB, if so return projection of P onto AB
    96.         float vc = d1 * d4 - d3 * d2;
    97.         if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
    98.         {
    99.             float v = d1 / (d1 - d3);
    100.             result = vertex1 + v * ab; //Barycentric coordinates (1-v,v,0)
    101.             return;
    102.         }
    103.  
    104.         //Check if P in vertex region outside C
    105.         Vector3 cp = point - vertex3;
    106.         float d5 = Vector3.Dot(ab, cp);
    107.         float d6 = Vector3.Dot(ac, cp);
    108.         if (d6 >= 0.0f && d5 <= d6)
    109.         {
    110.             result = vertex3; //Barycentric coordinates (0,0,1)
    111.             return;
    112.         }
    113.  
    114.         //Check if P in edge region of AC, if so return projection of P onto AC
    115.         float vb = d5 * d2 - d1 * d6;
    116.         if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
    117.         {
    118.             float w = d2 / (d2 - d6);
    119.             result = vertex1 + w * ac; //Barycentric coordinates (1-w,0,w)
    120.             return;
    121.         }
    122.  
    123.         //Check if P in edge region of BC, if so return projection of P onto BC
    124.         float va = d3 * d6 - d5 * d4;
    125.         if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
    126.         {
    127.             float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
    128.             result = vertex2 + w * (vertex3 - vertex2); //Barycentric coordinates (0,1-w,w)
    129.             return;
    130.         }
    131.  
    132.         //P inside face region. Compute Q through its barycentric coordinates (u,v,w)
    133.         float denom = 1.0f / (va + vb + vc);
    134.         float v2 = vb * denom;
    135.         float w2 = vc * denom;
    136.         result = vertex1 + ab * v2 + ac * w2; //= u*vertex1 + v*vertex2 + w*vertex3, u = va * denom = 1.0f - v - w
    137.     }
    138. }
    Code (CSharp):
    1.  
    2. using UnityEngine;
    3. using System.Collections.Generic;
    4.  
    5. public class KDTree
    6. {
    7.     public KDTree[] lr;
    8.     public Vector3 pivot;
    9.     public int pivotIndex;
    10.     public int axis;
    11.  
    12.     //    Change this value to 2 if you only need two-dimensional X,Y points. The search will
    13.     //    be quicker in two dimensions.
    14.     const int numDims = 3;
    15.  
    16.     public KDTree()
    17.     {
    18.         lr = new KDTree[2];
    19.     }
    20.  
    21.     //    Make a new tree from a list of points.
    22.     public static KDTree MakeFromPoints(params Vector3[] points)
    23.     {
    24.         int[] indices = Iota(points.Length);
    25.         return MakeFromPointsInner(0, 0, points.Length - 1, points, indices);
    26.     }
    27.  
    28.     //    Recursively build a tree by separating points at plane boundaries.
    29.     static KDTree MakeFromPointsInner(int depth, int stIndex, int enIndex, Vector3[] points, int[] inds)
    30.     {
    31.         KDTree root = new KDTree();
    32.         root.axis = depth % numDims;
    33.         int splitPoint = FindPivotIndex(points, inds, stIndex, enIndex, root.axis);
    34.  
    35.         root.pivotIndex = inds[splitPoint];
    36.         root.pivot = points[root.pivotIndex];
    37.  
    38.         int leftEndIndex = splitPoint - 1;
    39.  
    40.         if(leftEndIndex >= stIndex)
    41.         {
    42.             root.lr[0] = MakeFromPointsInner(depth + 1, stIndex, leftEndIndex, points, inds);
    43.         }
    44.  
    45.         int rightStartIndex = splitPoint + 1;
    46.  
    47.         if(rightStartIndex <= enIndex)
    48.         {
    49.             root.lr[1] = MakeFromPointsInner(depth + 1, rightStartIndex, enIndex, points, inds);
    50.         }
    51.  
    52.         return root;
    53.     }
    54.  
    55.     static void SwapElements(int[] arr, int a, int b)
    56.     {
    57.         int temp = arr[a];
    58.         arr[a] = arr[b];
    59.         arr[b] = temp;
    60.     }
    61.  
    62.     //    Simple "median of three" heuristic to find a reasonable splitting plane.
    63.     static int FindSplitPoint(Vector3[] points, int[] inds, int stIndex, int enIndex, int axis)
    64.     {
    65.         float a = points[inds[stIndex]][axis];
    66.         float b = points[inds[enIndex]][axis];
    67.         int midIndex = (stIndex + enIndex) / 2;
    68.         float m = points[inds[midIndex]][axis];
    69.  
    70.         if(a > b)
    71.         {
    72.             if(m > a)
    73.             {
    74.                 return stIndex;
    75.             }
    76.  
    77.             if(b > m)
    78.             {
    79.                 return enIndex;
    80.             }
    81.  
    82.             return midIndex;
    83.         }
    84.         else
    85.         {
    86.             if(a > m)
    87.             {
    88.                 return stIndex;
    89.             }
    90.  
    91.             if(m > b)
    92.             {
    93.                 return enIndex;
    94.             }
    95.  
    96.             return midIndex;
    97.         }
    98.     }
    99.  
    100.     //    Find a new pivot index from the range by splitting the points that fall either side
    101.     //    of its plane.
    102.     public static int FindPivotIndex(Vector3[] points, int[] inds, int stIndex, int enIndex, int axis)
    103.     {
    104.         int splitPoint = FindSplitPoint(points, inds, stIndex, enIndex, axis);
    105.         // int splitPoint = Random.Range(stIndex, enIndex);
    106.  
    107.         Vector3 pivot = points[inds[splitPoint]];
    108.         SwapElements(inds, stIndex, splitPoint);
    109.  
    110.         int currPt = stIndex + 1;
    111.         int endPt = enIndex;
    112.  
    113.         while(currPt <= endPt)
    114.         {
    115.             Vector3 curr = points[inds[currPt]];
    116.  
    117.             if((curr[axis] > pivot[axis]))
    118.             {
    119.                 SwapElements(inds, currPt, endPt);
    120.                 endPt--;
    121.             }
    122.             else
    123.             {
    124.                 SwapElements(inds, currPt - 1, currPt);
    125.                 currPt++;
    126.             }
    127.         }
    128.  
    129.         return currPt - 1;
    130.     }
    131.  
    132.     public static int[] Iota(int num)
    133.     {
    134.         int[] result = new int[num];
    135.  
    136.         for(int i = 0; i < num; i++)
    137.         {
    138.             result[i] = i;
    139.         }
    140.  
    141.         return result;
    142.     }
    143.  
    144.     //    Find the nearest point in the set to the supplied point.
    145.     public int FindNearest(Vector3 pt)
    146.     {
    147.         float bestSqDist = 1000000000f;
    148.         int bestIndex = -1;
    149.  
    150.         Search(pt, ref bestSqDist, ref bestIndex);
    151.  
    152.         return bestIndex;
    153.     }
    154.  
    155.     //    Recursively search the tree.
    156.     void Search(Vector3 pt, ref float bestSqSoFar, ref int bestIndex)
    157.     {
    158.         float mySqDist = (pivot - pt).sqrMagnitude;
    159.  
    160.         if(mySqDist < bestSqSoFar)
    161.         {
    162.             bestSqSoFar = mySqDist;
    163.             bestIndex = pivotIndex;
    164.         }
    165.  
    166.         float planeDist = pt[axis] - pivot[axis]; //DistFromSplitPlane(pt, pivot, axis);
    167.  
    168.         int selector = planeDist <= 0 ? 0 : 1;
    169.  
    170.         if(lr[selector] != null)
    171.         {
    172.             lr[selector].Search(pt, ref bestSqSoFar, ref bestIndex);
    173.         }
    174.  
    175.         selector = (selector + 1) % 2;
    176.  
    177.         float sqPlaneDist = planeDist * planeDist;
    178.  
    179.         if((lr[selector] != null) && (bestSqSoFar > sqPlaneDist))
    180.         {
    181.             lr[selector].Search(pt, ref bestSqSoFar, ref bestIndex);
    182.         }
    183.     }
    184.  
    185.     //Its possible for vertices the be in the exact same position, so we want to grab all of them.
    186.     public IList<int> FindNearestEpsilon(Vector3 pt, IList<int> resultBuffer) //Use result buffer to avoid garbage collection.
    187.     {
    188.         resultBuffer.Clear();
    189.  
    190.         float bestSqDist = 1000000000f;
    191.         int bestIndex = -1;
    192.  
    193.         SearchEpsilon(pt, ref bestSqDist, ref bestIndex, resultBuffer);
    194.  
    195.         return resultBuffer;
    196.     }
    197.  
    198.     void SearchEpsilon(Vector3 pt, ref float bestSqSoFar, ref int bestIndex, IList<int> resultBuffer)
    199.     {
    200.         float mySqDist = (pivot - pt).sqrMagnitude;
    201.  
    202.         if((mySqDist < bestSqSoFar || Mathf.Abs(mySqDist - bestSqSoFar) < Mathf.Epsilon))
    203.         {
    204.             if(mySqDist < bestSqSoFar + Mathf.Epsilon + Mathf.Epsilon) resultBuffer.Clear();
    205.  
    206.             bestSqSoFar = mySqDist;
    207.             bestIndex = pivotIndex;
    208.             resultBuffer.Add(pivotIndex);
    209.         }
    210.  
    211.         float planeDist = pt[axis] - pivot[axis]; //DistFromSplitPlane(pt, pivot, axis);
    212.  
    213.         int selector = planeDist <= 0 ? 0 : 1;
    214.  
    215.         if(lr[selector] != null)
    216.         {
    217.             lr[selector].SearchEpsilon(pt, ref bestSqSoFar, ref bestIndex, resultBuffer);
    218.         }
    219.  
    220.         selector = (selector + 1) % 2;
    221.  
    222.         float sqPlaneDist = planeDist * planeDist;
    223.  
    224.         if((lr[selector] != null) && (bestSqSoFar > sqPlaneDist))
    225.         {
    226.             lr[selector].SearchEpsilon(pt, ref bestSqSoFar, ref bestIndex, resultBuffer);
    227.         }
    228.     }
    229.  
    230.     //    Get a point's distance from an axis-aligned plane.
    231.     float DistFromSplitPlane(Vector3 pt, Vector3 planePt, int axis)
    232.     {
    233.         return pt[axis] - planePt[axis];
    234.     }
    235.  
    236.     //    Simple output of tree structure - mainly useful for getting a rough
    237.     //    idea of how deep the tree is (and therefore how well the splitting
    238.     //    heuristic is performing).
    239.     public string Dump(int level)
    240.     {
    241.         string result = pivotIndex.ToString().PadLeft(level) + "\n";
    242.  
    243.         if(lr[0] != null)
    244.         {
    245.             result += lr[0].Dump(level + 2);
    246.         }
    247.  
    248.         if(lr[1] != null)
    249.         {
    250.             result += lr[1].Dump(level + 2);
    251.         }
    252.  
    253.         return result;
    254.     }
    255. }
    Code (CSharp):
    1.  
    2. using UnityEngine;
    3. using System.Collections;
    4.  
    5. //    This is a list recording which triangles use each vertex. It is essentially
    6. //    a jagged array with one top level entry per vertex. Each of the separate
    7. //    sub-arrays is a list of all the triangles that use that vertex.
    8. public class VertTriList
    9. {
    10.     public int[][] list;
    11.  
    12.     //    Indexable - use "vertTri[i]" to get the list of triangles for vertex i.
    13.     public int[] this[int index]
    14.     {
    15.         get { return list[index]; }
    16.     }
    17.  
    18.     public VertTriList(int[] tri, int numVerts)
    19.     {
    20.         Init(tri, numVerts);
    21.     }
    22.  
    23.     public VertTriList(Mesh mesh)
    24.     {
    25.         Init(mesh.triangles, mesh.vertexCount);
    26.     }
    27.  
    28.     //    You don't usually need to call this - it's just to assist the implementation
    29.     //    of the constructors.
    30.     public void Init(int[] tri, int numVerts)
    31.     {
    32.         //    First, go through the triangles, keeping a count of how many times
    33.         //    each vert is used.
    34.         int[] counts = new int[numVerts];
    35.  
    36.         for(int i = 0; i < tri.Length; i++)
    37.         {
    38.             counts[tri[i]]++;
    39.         }
    40.  
    41.         //    Initialise an empty jagged array with the appropriate number of elements
    42.         //    for each vert.
    43.         list = new int[numVerts][];
    44.  
    45.         for(int i = 0; i < counts.Length; i++)
    46.         {
    47.             list[i] = new int[counts[i]];
    48.         }
    49.  
    50.         //    Assign the appropriate triangle number each time a given vert
    51.         //    is encountered in the triangles.
    52.         for(int i = 0; i < tri.Length; i++)
    53.         {
    54.             int vert = tri[i];
    55.             list[vert][--counts[vert]] = i / 3;
    56.         }
    57.     }
    58. }

    I will also notify Iron-Warrior (creator of the BSPTree, SuperCollider and such.. as shown in a previous post) so maybe he can have a look at the KDTree.
     
    Last edited: Apr 20, 2016
  16. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    The problem with that kind of script, is that you have to put it on every mesh collider that you want to be able to collide with, and it is not very efficient. I'm now using an array of RayCasts, and it works very well without impacting my performance.
     
  17. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    The problem with just using a bunch of raycasts is that it might not be accurate enough for things like precise character collisions.
    If it works for you then that is fine =), I am more so just posting stuff for future people who might need it.
    I am working on fixing the KDTree bug, however, just out of curiosity maybe try the code I posted above and see how the performance is compared to the BSPTree.
    Edit - fixed the KDTree bug.
     
    Last edited: Apr 4, 2016
  18. Moddingear

    Moddingear

    Joined:
    Apr 1, 2016
    Posts:
    29
    I can give you a map for you to test, but right now my script isn't compatible anymore with SphereCast and OverlapSphere
    I'm using that code to get the point in a sphere multiple times, then I raycast to where I want :
    Code (CSharp):
    1. Vector3 Circle(float radius, float Angle)
    2.     {
    3.         Vector3 pos;
    4.         pos.x = radius * Mathf.Sin(Angle * Mathf.Deg2Rad);
    5.         pos.y = 0;
    6.         pos.z = radius * Mathf.Cos(Angle * Mathf.Deg2Rad);
    7.         return pos;
    8.     }
     
  19. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    Its fine, I was just curious is all =)
     
  20. invadererik

    invadererik

    Joined:
    Oct 31, 2010
    Posts:
    148
    @HiddenMonk any updates ? Whats the best option: KD Tree, BSP Tree, AABB Tree ?
     
  21. PhilSA

    PhilSA

    Joined:
    Jul 11, 2013
    Posts:
    1,926
    If we're really just talking about an overlapSphere, there's a simple way to calculate this:
    1. Do an OverlapSphere() to find all overlapped colliders
    2. for each overlapped colliders do a ComputePenetration() with your sphere
    3. the collision point is the center of your sphere + (penetrationNormal * (sphereRadius - penetrationDistance)) !
    However, there's been talks by Unity devs of exposing PhysX's immediate mode after PhysX3.4 is integrated. This would allow you to directly find contacts between any two colliders at any time
     
    Last edited: Jan 24, 2018
  22. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    Try what PhilSA suggested, or just have a look at unitys Physics.ClosestPoint and Physics.ComputePenetration in general.

    However, what I did before these methods came out was make this
    https://github.com/HiddenMonk/Unity3DCustomCharacterControllerCapsuleCollisionDetection

    I use a AABB tree found in my project here. The AABB tree might be fine for when your only touching a few triangles of the mesh, but if you are touching a lot of triangles of the mesh then this is really slow. I dont know how physx handles it.
    https://github.com/HiddenMonk/Unity...llision/CCCollision/Collision/MeshAABBTree.cs

    Here are the Closest points methods
    https://github.com/HiddenMonk/Unity...eCharacterCollision/Extensions/ExtCollider.cs

    And here are the DetectCollisions methods for finding all contacts (sphere and capsule version)
    https://github.com/HiddenMonk/Unity...CCollision/Collision/SphereCollisionDetect.cs
     
  23. invadererik

    invadererik

    Joined:
    Oct 31, 2010
    Posts:
    148
    @PhilSA Thanks ! ComputePenetration has one slight issue in that it takes a Collider as input ...

    @HiddenMonk , so did you find that the AABB tree performed better than the KDTree ? Or did the KDTree have issues with the overlapping vertices so AABB was better choice albeit slower ?
     
  24. HiddenMonk

    HiddenMonk

    Joined:
    Dec 19, 2014
    Posts:
    987
    From what I remember, and based on what I said in an edit above, the KDTree was not accurate.
    What I wrote was
    "The reason the KDTree seems to perform better than the bsptree is because the kdtree is only searching for the closest vertex and not all the triangles that are touching the sphere. "
    So basically, the KDTree is pretty much useless, at least how I was using it.
    Here is a image showing the issue of the KDTree
    BadKDTree.png

    So all I was left with was using the BSPTree or the AABB tree, and I went with the AABB tree since it got rid of the duplicating triangle issue and I think it was faster, but I cant remember. Its what I am using now so I assume it was better. Of course its possible that the BSPTree maker and the AABB tree I made using the BSPTree as the starting point might be poorly designed and there may be better ways of handling this.

    However, there has to be something more that needs to be done to be able to find multiple contact points with better performance. Physx runs much faster than what I got, so I dont know what extra thing they might be doing. They might be behind the scenes splitting a concave mesh into multiple convex meshes and using algorithms like GJK or whatever.

    Also, keep in mind that the AABB Mesh code basically treats every mesh as Concave, so even if you have the collider marked as Convex it will still treat it as concave. The reason for this is because unity doesnt give us the Convex hulls vertices, so there is no way for me to generate the Tree =/.
     
    Last edited: Jan 26, 2018
  25. invadererik

    invadererik

    Joined:
    Oct 31, 2010
    Posts:
    148
    @HiddenMonk thanks, the image really helped to explain the issue with the KDTree !
     
    HiddenMonk likes this.
  26. bojidarvelikov2

    bojidarvelikov2

    Joined:
    Mar 23, 2019
    Posts:
    4
    Hi, you can try this, if you are still looking for a solution. There is method in the Collider class called ClosestPoint(Vector3 position) that returns the closest point on the collider to the given point. You can pass the sphere center to this function and it will return the location of the closest point on the collider to the sphere center. From here, you have the two points and you can easily find the distance between them.