1cea1d02f083a7d9ace7bb6d3087a4b461cfa77e
[blender.git] / extern / bullet / Bullet / CollisionShapes / ConvexHullShape.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10  */
11
12 #include "ConvexHullShape.h"
13 #include "NarrowPhaseCollision/CollisionMargin.h"
14
15 #include "SimdQuaternion.h"
16
17
18 ConvexHullShape ::ConvexHullShape (SimdPoint3* points,int numPoints)
19 {
20         m_points.resize(numPoints);
21         for (int i=0;i<numPoints;i++)
22                 m_points[i] = points[i];
23 }
24
25 SimdVector3     ConvexHullShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
26 {
27         SimdVector3 supVec(0.f,0.f,0.f);
28         SimdScalar newDot,maxDot = -1e30f;
29
30         SimdVector3 vec = vec0;
31         SimdScalar lenSqr = vec.length2();
32         if (lenSqr < 0.0001f)
33         {
34                 vec.setValue(1,0,0);
35         } else
36         {
37                 float rlen = 1.f / SimdSqrt(lenSqr );
38                 vec *= rlen;
39         }
40
41
42         for (int i=0;i<m_points.size();i++)
43         {
44                 SimdPoint3 vtx = m_points[i] * m_localScaling;
45
46                 newDot = vec.dot(vtx);
47                 if (newDot > maxDot)
48                 {
49                         maxDot = newDot;
50                         supVec = vtx;
51                 }
52         }
53         return supVec;
54 }
55
56
57 SimdVector3     ConvexHullShape::LocalGetSupportingVertex(const SimdVector3& vec)const
58 {
59         SimdVector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
60
61         if ( GetMargin()!=0.f )
62         {
63                 SimdVector3 vecnorm = vec;
64                 if (vecnorm .length2() == 0.f)
65                 {
66                         vecnorm.setValue(-1.f,-1.f,-1.f);
67                 } 
68                 vecnorm.normalize();
69                 supVertex+= GetMargin() * vecnorm;
70         }
71         return supVertex;
72 }
73
74
75
76
77
78 void    ConvexHullShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
79 {
80         //not yet, return box inertia
81
82         float margin = GetMargin();
83
84         SimdTransform ident;
85         ident.setIdentity();
86         SimdVector3 aabbMin,aabbMax;
87         GetAabb(ident,aabbMin,aabbMax);
88         SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f;
89
90         SimdScalar lx=2.f*(halfExtents.x()+margin);
91         SimdScalar ly=2.f*(halfExtents.y()+margin);
92         SimdScalar lz=2.f*(halfExtents.z()+margin);
93         const SimdScalar x2 = lx*lx;
94         const SimdScalar y2 = ly*ly;
95         const SimdScalar z2 = lz*lz;
96         const SimdScalar scaledmass = mass * 0.08333333f;
97
98         inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
99
100 }
101
102
103
104 //currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection
105 //Please note that you can debug-draw ConvexHullShape with the Raytracer Demo
106 int     ConvexHullShape::GetNumVertices() const
107 {
108         return m_points.size();
109 }
110
111 int ConvexHullShape::GetNumEdges() const
112 {
113         return m_points.size()*m_points.size();
114 }
115
116 void ConvexHullShape::GetEdge(int i,SimdPoint3& pa,SimdPoint3& pb) const
117 {
118
119         int index0 = i%m_points.size();
120         int index1 = i/m_points.size();
121         pa = m_points[index0]*m_localScaling;
122         pb = m_points[index1]*m_localScaling;
123 }
124
125 void ConvexHullShape::GetVertex(int i,SimdPoint3& vtx) const
126 {
127         vtx = m_points[i]*m_localScaling;
128 }
129
130 int     ConvexHullShape::GetNumPlanes() const
131 {
132         return 0;
133 }
134
135 void ConvexHullShape::GetPlane(SimdVector3& planeNormal,SimdPoint3& planeSupport,int i ) const
136 {
137         assert(0);
138 }
139
140 //not yet
141 bool ConvexHullShape::IsInside(const SimdPoint3& pt,SimdScalar tolerance) const
142 {
143         assert(0);
144         return false;
145 }
146