Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / BulletDynamics / Dynamics / RigidBody.cpp
1 #include "RigidBody.h"
2 #include "MassProps.h"
3 #include "CollisionShapes/ConvexShape.h"
4 #include "GEN_MinMax.h"
5 #include <SimdTransformUtil.h>
6
7 float gLinearAirDamping = 1.f;
8
9 static int uniqueId = 0;
10
11 RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
12 : m_collisionShape(0),
13         m_activationState1(1),
14         m_deactivationTime(0.f),
15         m_hitFraction(1.f),
16         m_gravity(0.0f, 0.0f, 0.0f),
17         m_linearDamping(0.f),
18         m_angularDamping(0.5f),
19         m_totalForce(0.0f, 0.0f, 0.0f),
20         m_totalTorque(0.0f, 0.0f, 0.0f),
21         m_linearVelocity(0.0f, 0.0f, 0.0f),
22         m_angularVelocity(0.f,0.f,0.f),
23         m_restitution(restitution),
24         m_friction(friction)
25 {
26         m_debugBodyId = uniqueId++;
27         
28         setMassProps(massProps.m_mass, massProps.m_inertiaLocal);
29     setDamping(linearDamping, angularDamping);
30         m_worldTransform.setIdentity();
31         updateInertiaTensor();
32
33 }
34
35 void RigidBody::activate()
36 {
37                 SetActivationState(1);
38                 m_deactivationTime = 0.f;
39 }
40 void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
41
42
43         m_linearVelocity = lin_vel; 
44 }
45
46
47 void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const
48 {
49         SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
50 }
51
52         
53 void    RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
54 {
55         m_collisionShape ->GetAabb(m_worldTransform,aabbMin,aabbMax);
56 }
57
58 void    RigidBody::SetCollisionShape(CollisionShape* mink)
59 {
60         m_collisionShape = mink;
61 }
62
63
64 void RigidBody::setGravity(const SimdVector3& acceleration) 
65 {
66         if (m_inverseMass != 0.0f)
67         {
68                 m_gravity = acceleration * (1.0f / m_inverseMass);
69         }
70 }
71
72 bool RigidBody::mergesSimulationIslands() const
73 {
74         return ( getInvMass() != 0) ;
75 }
76
77 void RigidBody::SetActivationState(int newState) 
78
79         m_activationState1 = newState;
80 }
81
82
83
84 void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
85 {
86         m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
87         m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
88 }
89
90
91
92 #include <stdio.h>
93
94
95 void RigidBody::applyForces(SimdScalar step)
96 {
97         
98         applyCentralForce(m_gravity);   
99         
100         m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
101         m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
102         
103 }
104
105 void RigidBody::proceedToTransform(const SimdTransform& newTrans)
106 {
107         setCenterOfMassTransform( newTrans );
108 }
109         
110
111 void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
112 {
113         m_inverseMass = mass != 0.0f ? 1.0f / mass : 0.0f;
114         m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
115                                                    inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
116                                                    inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
117
118 }
119
120         
121
122 void RigidBody::updateInertiaTensor() 
123 {
124         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
125 }
126
127
128 void RigidBody::integrateVelocities(SimdScalar step) 
129 {
130         m_linearVelocity += m_totalForce * (m_inverseMass * step);
131         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
132
133 #define MAX_ANGVEL SIMD_HALF_PI
134         /// clamp angular velocity. collision calculations will fail on higher angular velocities       
135         float angvel = m_angularVelocity.length();
136         if (angvel*step > MAX_ANGVEL)
137         {
138                 m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
139         }
140
141         clearForces();
142 }
143
144 SimdQuaternion RigidBody::getOrientation() const
145 {
146                 SimdQuaternion orn;
147                 m_worldTransform.getBasis().getRotation(orn);
148                 return orn;
149 }
150         
151         
152 void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
153 {
154         m_worldTransform = xform;
155         SimdQuaternion orn;
156 //      m_worldTransform.getBasis().getRotation(orn);
157 //      orn.normalize();
158 //      m_worldTransform.setBasis(SimdMatrix3x3(orn));
159
160 //      m_worldTransform.getBasis().getRotation(m_orn1);
161         updateInertiaTensor();
162 }
163
164
165