1cc6226360354b38d2826f4c2f41b1fb56c9482f
[blender-staging.git] / extern / bullet / Bullet / CollisionShapes / BoxShape.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
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 "BoxShape.h"
13
14 SimdVector3 BoxShape::GetHalfExtents() const
15 {
16         return m_boxHalfExtents1 * m_localScaling;
17 }
18 //{ 
19
20
21 void BoxShape::GetAabb(const SimdTransform& t,SimdVector3& aabbMin,SimdVector3& aabbMax) const
22 {
23         SimdVector3 halfExtents = GetHalfExtents();
24
25         SimdMatrix3x3 abs_b = t.getBasis().absolute();  
26         SimdPoint3 center = t.getOrigin();
27         SimdVector3 extent = SimdVector3(abs_b[0].dot(halfExtents),
28                    abs_b[1].dot(halfExtents),
29                   abs_b[2].dot(halfExtents));
30         extent += SimdVector3(GetMargin(),GetMargin(),GetMargin());
31
32
33         //todo: this is a quick fix, we need to enlarge the aabb dependent on several criteria
34         //extent += SimdVector3(.2f,.2f,.2f);
35
36         aabbMin = center - extent;
37         aabbMax = center + extent;
38
39
40 }
41
42
43 void    BoxShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
44 {
45         float margin = 0.f;
46         SimdVector3 halfExtents = GetHalfExtents();
47
48         SimdScalar lx=2.f*(halfExtents.x());
49         SimdScalar ly=2.f*(halfExtents.y());
50         SimdScalar lz=2.f*(halfExtents.z());
51
52         inertia[0] = mass/(12.0f) * (ly*ly + lz*lz);
53         inertia[1] = mass/(12.0f) * (lx*lx + lz*lz);
54         inertia[2] = mass/(12.0f) * (lx*lx + ly*ly);
55
56
57 //              float radius = GetHalfExtents().length();
58 //              SimdScalar elem = 0.4f * mass * radius*radius;
59 //      inertia[0] = inertia[1] = inertia[2] = elem;
60 return;
61 /*
62         float margin = GetMargin();
63         SimdVector3 halfExtents = GetHalfExtents();
64
65         SimdScalar lx=2.f*(halfExtents.x()+margin);
66         SimdScalar ly=2.f*(halfExtents.y()+margin);
67         SimdScalar lz=2.f*(halfExtents.z()+margin);
68         const SimdScalar x2 = lx*lx;
69         const SimdScalar y2 = ly*ly;
70         const SimdScalar z2 = lz*lz;
71         const SimdScalar scaledmass = mass * 0.08333333f;
72
73         inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2));
74 */
75 //              inertia.x() = scaledmass * (y2+z2);
76 //              inertia.y() = scaledmass * (x2+z2);
77 //              inertia.z() = scaledmass * (x2+y2);
78 }