Newton Dynamics  4.00
ndContactSolver.h
1 /* Copyright (c) <2003-2022> <Julio Jerez, Newton Game Dynamics>
2 *
3 * This software is provided 'as-is', without any express or implied
4 * warranty. In no event will the authors be held liable for any damages
5 * arising from the use of this software.
6 *
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 *
11 * 1. The origin of this software must not be misrepresented; you must not
12 * claim that you wrote the original software. If you use this software
13 * in a product, an acknowledgment in the product documentation would be
14 * appreciated but is not required.
15 *
16 * 2. Altered source versions must be plainly marked as such, and must not be
17 * misrepresented as being the original software.
18 *
19 * 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 #ifndef __ND_CONTACT_SOLVER_H__
23 #define __ND_CONTACT_SOLVER_H__
24 
25 #include "ndCollisionStdafx.h"
26 #include "ndShape.h"
27 #include "ndShapeInstance.h"
28 
29 class ndPlane;
30 class ndBodyKinematic;
31 class ndContactNotify;
32 class ndPolygonMeshDesc;
33 
34 D_MSV_NEWTON_ALIGN_32
36 {
37  public:
38  ndPlane m_plane;
39  ndMinkFace* m_twin[3];
40  ndInt16 m_vertex[3];
41  ndInt8 m_mark;
42  ndInt8 m_alive;
43 } D_GCC_NEWTON_ALIGN_32 ;
44 
45 #define D_SEPARATION_PLANES_ITERATIONS 8
46 #define D_CONVEX_MINK_STACK_SIZE 64
47 #define D_CONNICS_CONTATS_ITERATIONS 32
48 #define D_CONVEX_MINK_MAX_FACES 512
49 #define D_CONVEX_MINK_MAX_POINTS 256
50 #define D_MAX_EDGE_COUNT 2048
51 #define D_PENETRATION_TOL ndFloat32 (1.0f / 1024.0f)
52 #define D_MINK_VERTEX_ERR ndFloat32 (1.0e-3f)
53 #define D_MINK_VERTEX_ERR2 (D_MINK_VERTEX_ERR * D_MINK_VERTEX_ERR)
54 
55 class ndContact;
56 class dCollisionParamProxy;
57 
58 D_MSV_NEWTON_ALIGN_32
59 class ndContactSolver: public ndDownHeap<ndMinkFace *, ndFloat32>
60 {
61  public:
62  class ndBoxBoxDistance2;
63 
64  D_COLLISION_API ndContactSolver();
65  ~ndContactSolver() {}
66 
67  D_COLLISION_API void CalculateContacts(
68  const ndShapeInstance* const shapeA, const ndMatrix& matrixA, const ndVector& velocA,
69  const ndShapeInstance* const shapeB, const ndMatrix& matrixB, const ndVector& velocB,
70  ndFixSizeArray<ndContactPoint, 16>& contactOut, ndContactNotify* const notification);
71 
72  private:
73  ndContactSolver(ndContact* const contact, ndContactNotify* const notification, ndFloat32 timestep, ndInt32 threadId);
74  ndContactSolver(ndShapeInstance* const instance, ndContactNotify* const notification, ndFloat32 timestep, ndInt32 threadId);
75  ndContactSolver(const ndContactSolver& src, const ndShapeInstance& instance0, const ndShapeInstance& instance1);
76 
77  ndInt32 CalculateContactsDiscrete(); // done
78  ndInt32 CalculateContactsContinue(); // done
79  ndFloat32 RayCast (const ndVector& localP0, const ndVector& localP1, ndContactPoint& contactOut);
80 
81  ndInt32 ConvexContactsDiscrete(); // done
82  ndInt32 CompoundContactsDiscrete(); // done
83  ndInt32 ConvexToConvexContactsDiscrete(); // done
84  ndInt32 CompoundToStaticProceduralMesh(); // done
85  ndInt32 ConvexToCompoundContactsDiscrete(); // done
86  ndInt32 CompoundToConvexContactsDiscrete(); // done
87  ndInt32 CompoundToCompoundContactsDiscrete(); // done
88  ndInt32 ConvexToStaticMeshContactsDiscrete(); // done
89  ndInt32 CompoundToShapeStaticBvhContactsDiscrete(); // done
90  ndInt32 CompoundToStaticHeightfieldContactsDiscrete(); // done
91  ndInt32 CalculatePolySoupToHullContactsDescrete(ndPolygonMeshDesc& data); // done
92  ndInt32 ConvexToSaticStaticBvhContactsNodeDescrete(const ndAabbPolygonSoup::ndNode* const node); // done
93 
94  ndInt32 ConvexContactsContinue(); // done
95  ndInt32 CompoundContactsContinue(); // done
96  ndInt32 ConvexToConvexContactsContinue(); // done
97  ndInt32 ConvexToCompoundContactsContinue(); // done
98  ndInt32 ConvexToStaticMeshContactsContinue(); // done
99  ndInt32 CalculatePolySoupToHullContactsContinue(ndPolygonMeshDesc& data); // done
100 
101  class dgPerimenterEdge
102  {
103  public:
104  const ndVector* m_vertex;
105  dgPerimenterEdge* m_next;
106  dgPerimenterEdge* m_prev;
107  };
108 
109  class dgFaceFreeList
110  {
111  public:
112  dgFaceFreeList* m_next;
113  };
114 
115  inline ndMinkFace* NewFace();
116  inline void PushFace(ndMinkFace* const face);
117  inline void DeleteFace(ndMinkFace* const face);
118  inline ndMinkFace* AddFace(ndInt32 v0, ndInt32 v1, ndInt32 v2);
119 
120  bool CalculateClosestPoints();
121  ndInt32 CalculateClosestSimplex();
122 
123  ndInt32 CalculateIntersectingPlane(ndInt32 count);
124  ndInt32 PruneContacts(ndInt32 count, ndInt32 maxCount) const;
125  ndInt32 PruneSupport(ndInt32 count, const ndVector& dir, const ndVector* const points) const;
126  ndInt32 CalculateContacts(const ndVector& point0, const ndVector& point1, const ndVector& normal);
127  ndInt32 Prune2dContacts(const ndMatrix& matrix, ndInt32 count, ndContactPoint* const contactArray, ndInt32 maxCount) const;
128  ndInt32 Prune3dContacts(const ndMatrix& matrix, ndInt32 count, ndContactPoint* const contactArray, ndInt32 maxCount) const;
129  ndInt32 ConvexPolygonsIntersection(const ndVector& normal, ndInt32 count1, ndVector* const shape1, ndInt32 count2, ndVector* const shape2, ndVector* const contactOut, ndInt32 maxContacts) const;
130  ndInt32 ConvexPolygonToLineIntersection(const ndVector& normal, ndInt32 count1, ndVector* const shape1, ndInt32 count2, ndVector* const shape2, ndVector* const contactOut, ndVector* const mem) const;
131 
132  ndBigVector ReduceLine(ndInt32& indexOut);
133  ndBigVector ReduceTriangle (ndInt32& indexOut);
134  ndBigVector ReduceTetrahedrum (ndInt32& indexOut);
135  void SupportVertex(const ndVector& dir, ndInt32 vertexIndex);
136 
137  void TranslateSimplex(const ndVector& step);
138  void CalculateContactFromFeacture(ndInt32 featureType);
139 
140  ndShapeInstance m_instance0;
141  ndShapeInstance m_instance1;
142  ndVector m_closestPoint0;
143  ndVector m_closestPoint1;
144  ndVector m_separatingVector;
145  union
146  {
147  ndVector m_buffer[2 * D_CONVEX_MINK_MAX_POINTS];
148  struct
149  {
150  ndVector m_hullDiff[D_CONVEX_MINK_MAX_POINTS];
151  ndVector m_hullSum[D_CONVEX_MINK_MAX_POINTS];
152  };
153  };
154 
155  ndContact* m_contact;
156  dgFaceFreeList* m_freeFace;
157  ndContactNotify* m_notification;
158  ndContactPoint* m_contactBuffer;
159  ndFloat32 m_timestep;
160  ndFloat32 m_skinMargin;
161  ndFloat32 m_separationDistance;
162 
163  ndInt32 m_threadId;
164  ndInt32 m_maxCount;
165  ndInt32 m_faceIndex;
166  ndInt32 m_vertexIndex;
167  ndUnsigned32 m_pruneContacts : 1;
168  ndUnsigned32 m_intersectionTestOnly : 1;
169 
170  ndMinkFace* m_faceStack[D_CONVEX_MINK_STACK_SIZE];
171  ndMinkFace* m_coneFaceList[D_CONVEX_MINK_STACK_SIZE];
172  ndMinkFace* m_deletedFaceList[D_CONVEX_MINK_STACK_SIZE];
173  ndMinkFace m_facePool[D_CONVEX_MINK_MAX_FACES];
174  ndInt8 m_heapBuffer[D_CONVEX_MINK_MAX_FACES * (sizeof (ndFloat32) + sizeof (ndMinkFace *))];
175 
176  static ndVector m_pruneUpDir;
177  static ndVector m_pruneSupportX;
178 
179  static ndVector m_hullDirs[14];
180  static ndInt32 m_rayCastSimplex[4][4];
181 
182  friend class ndScene;
183  friend class ndShapeConvex;
184  friend class ndShapeInstance;
185  friend class ndPolygonMeshDesc;
186  friend class ndConvexCastNotify;
187  friend class ndShapeConvexPolygon;
189 } D_GCC_NEWTON_ALIGN_32;
190 
191 #endif
192 
193 
ndPolygonMeshDesc
Definition: ndPolygonMeshDesc.h:35
ndScene
Definition: ndScene.h:59
ndFixSizeArray
Definition: ndFixSizeArray.h:29
ndDownHeap
Definition: ndHeap.h:346
ndContactNotify
Definition: ndContactNotify.h:62
ndContact
Definition: ndContact.h:97
ndShapeConvexPolygon
Definition: ndShapeConvexPolygon.h:32
ndContactSolver
Definition: ndContactSolver.h:60
ndPlane
Definition: ndPlane.h:35
ndBodyPlayerCapsuleContactSolver
Definition: ndBodyPlayerCapsule.cpp:43
ndShapeConvex
Definition: ndShapeConvex.h:34
ndContactPoint
Definition: ndContact.h:39
ndBodyKinematic
Definition: ndBodyKinematic.h:40
ndAabbPolygonSoup::ndNode
Definition: ndAabbPolygonSoup.h:58
ndContactSolver::ndBoxBoxDistance2
Definition: ndContactSolver.cpp:70
ndMatrix
Definition: ndMatrix.h:42
ndBigVector
Definition: ndVectorArmNeon.h:463
ndMinkFace
Definition: ndContactSolver.h:36
ndConvexCastNotify
Definition: ndConvexCastNotify.h:34
ndShapeInstance
Definition: ndShapeInstance.h:62
ndVector
Definition: ndVectorArmNeon.h:41