dali-physics/third-party/bullet3/src/BulletCollision/Gimpact/btTriangleShapeEx.cpp

   1 /*! \file btGImpactTriangleShape.h
   2 \author Francisco Leon Najera
   3 */
   4 /*
   5 This source file is part of GIMPACT Library.
   6
   7 For the latest info, see http://gimpact.sourceforge.net/
   8
   9 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
  10 email: projectileman@yahoo.com
  11
  12
  13 This software is provided 'as-is', without any express or implied warranty.
  14 In no event will the authors be held liable for any damages arising from the use of this software.
  15 Permission is granted to anyone to use this software for any purpose,
  16 including commercial applications, and to alter it and redistribute it freely,
  17 subject to the following restrictions:
  18
  19 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  20 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  21 3. This notice may not be removed or altered from any source distribution.
  22 */
  23
  24 #include "btTriangleShapeEx.h"
  25
  26 void GIM_TRIANGLE_CONTACT::merge_points(const btVector4& plane,
  27                                                                                 btScalar margin, const btVector3* points, int point_count)
  28 {
  29         m_point_count = 0;
  30         m_penetration_depth = -1000.0f;
  31
  32         int point_indices[MAX_TRI_CLIPPING];
  33
  34         int _k;
  35
  36         for (_k = 0; _k < point_count; _k++)
  37         {
  38                 btScalar _dist = -bt_distance_point_plane(plane, points[_k]) + margin;
  39
  40                 if (_dist >= 0.0f)
  41                 {
  42                         if (_dist > m_penetration_depth)
  43                         {
  44                                 m_penetration_depth = _dist;
  45                                 point_indices[0] = _k;
  46                                 m_point_count = 1;
  47                         }
  48                         else if ((_dist + SIMD_EPSILON) >= m_penetration_depth)
  49                         {
  50                                 point_indices[m_point_count] = _k;
  51                                 m_point_count++;
  52                         }
  53                 }
  54         }
  55
  56         for (_k = 0; _k < m_point_count; _k++)
  57         {
  58                 m_points[_k] = points[point_indices[_k]];
  59         }
  60 }
  61
  62 ///class btPrimitiveTriangle
  63 bool btPrimitiveTriangle::overlap_test_conservative(const btPrimitiveTriangle& other)
  64 {
  65         btScalar total_margin = m_margin + other.m_margin;
  66         // classify points on other triangle
  67         btScalar dis0 = bt_distance_point_plane(m_plane, other.m_vertices[0]) - total_margin;
  68
  69         btScalar dis1 = bt_distance_point_plane(m_plane, other.m_vertices[1]) - total_margin;
  70
  71         btScalar dis2 = bt_distance_point_plane(m_plane, other.m_vertices[2]) - total_margin;
  72
  73         if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
  74
  75         // classify points on this triangle
  76         dis0 = bt_distance_point_plane(other.m_plane, m_vertices[0]) - total_margin;
  77
  78         dis1 = bt_distance_point_plane(other.m_plane, m_vertices[1]) - total_margin;
  79
  80         dis2 = bt_distance_point_plane(other.m_plane, m_vertices[2]) - total_margin;
  81
  82         if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
  83
  84         return true;
  85 }
  86
  87 int btPrimitiveTriangle::clip_triangle(btPrimitiveTriangle& other, btVector3* clipped_points)
  88 {
  89         // edge 0
  90
  91         btVector3 temp_points[MAX_TRI_CLIPPING];
  92
  93         btVector4 edgeplane;
  94
  95         get_edge_plane(0, edgeplane);
  96
  97         int clipped_count = bt_plane_clip_triangle(
  98                 edgeplane, other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], temp_points);
  99
 100         if (clipped_count == 0) return 0;
 101
 102         btVector3 temp_points1[MAX_TRI_CLIPPING];
 103
 104         // edge 1
 105         get_edge_plane(1, edgeplane);
 106
 107         clipped_count = bt_plane_clip_polygon(edgeplane, temp_points, clipped_count, temp_points1);
 108
 109         if (clipped_count == 0) return 0;
 110
 111         // edge 2
 112         get_edge_plane(2, edgeplane);
 113
 114         clipped_count = bt_plane_clip_polygon(
 115                 edgeplane, temp_points1, clipped_count, clipped_points);
 116
 117         return clipped_count;
 118 }
 119
 120 bool btPrimitiveTriangle::find_triangle_collision_clip_method(btPrimitiveTriangle& other, GIM_TRIANGLE_CONTACT& contacts)
 121 {
 122         btScalar margin = m_margin + other.m_margin;
 123
 124         btVector3 clipped_points[MAX_TRI_CLIPPING];
 125         int clipped_count;
 126         //create planes
 127         // plane v vs U points
 128
 129         GIM_TRIANGLE_CONTACT contacts1;
 130
 131         contacts1.m_separating_normal = m_plane;
 132
 133         clipped_count = clip_triangle(other, clipped_points);
 134
 135         if (clipped_count == 0)
 136         {
 137                 return false;  //Reject
 138         }
 139
 140         //find most deep interval face1
 141         contacts1.merge_points(contacts1.m_separating_normal, margin, clipped_points, clipped_count);
 142         if (contacts1.m_point_count == 0) return false;  // too far
 143         //Normal pointing to this triangle
 144         contacts1.m_separating_normal *= -1.f;
 145
 146         //Clip tri1 by tri2 edges
 147         GIM_TRIANGLE_CONTACT contacts2;
 148         contacts2.m_separating_normal = other.m_plane;
 149
 150         clipped_count = other.clip_triangle(*this, clipped_points);
 151
 152         if (clipped_count == 0)
 153         {
 154                 return false;  //Reject
 155         }
 156
 157         //find most deep interval face1
 158         contacts2.merge_points(contacts2.m_separating_normal, margin, clipped_points, clipped_count);
 159         if (contacts2.m_point_count == 0) return false;  // too far
 160
 161         ////check most dir for contacts
 162         if (contacts2.m_penetration_depth < contacts1.m_penetration_depth)
 163         {
 164                 contacts.copy_from(contacts2);
 165         }
 166         else
 167         {
 168                 contacts.copy_from(contacts1);
 169         }
 170         return true;
 171 }
 172
 173 ///class btTriangleShapeEx: public btTriangleShape
 174
 175 bool btTriangleShapeEx::overlap_test_conservative(const btTriangleShapeEx& other)
 176 {
 177         btScalar total_margin = getMargin() + other.getMargin();
 178
 179         btVector4 plane0;
 180         buildTriPlane(plane0);
 181         btVector4 plane1;
 182         other.buildTriPlane(plane1);
 183
 184         // classify points on other triangle
 185         btScalar dis0 = bt_distance_point_plane(plane0, other.m_vertices1[0]) - total_margin;
 186
 187         btScalar dis1 = bt_distance_point_plane(plane0, other.m_vertices1[1]) - total_margin;
 188
 189         btScalar dis2 = bt_distance_point_plane(plane0, other.m_vertices1[2]) - total_margin;
 190
 191         if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 192
 193         // classify points on this triangle
 194         dis0 = bt_distance_point_plane(plane1, m_vertices1[0]) - total_margin;
 195
 196         dis1 = bt_distance_point_plane(plane1, m_vertices1[1]) - total_margin;
 197
 198         dis2 = bt_distance_point_plane(plane1, m_vertices1[2]) - total_margin;
 199
 200         if (dis0 > 0.0f && dis1 > 0.0f && dis2 > 0.0f) return false;
 201
 202         return true;
 203 }