Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / Extras / PhysicsInterface / CcdPhysics / CcdPhysicsController.cpp
1 #include "CcdPhysicsController.h"
2
3 #include "Dynamics/RigidBody.h"
4 #include "PHY_IMotionState.h"
5 #include "BroadphaseCollision/BroadphaseProxy.h"
6 #include "CollisionShapes/ConvexShape.h"
7
8 class BP_Proxy;
9
10 ///todo: fill all the empty CcdPhysicsController methods, hook them up to the RigidBody class
11
12 //'temporarily' global variables
13 float   gDeactivationTime = 2.f;
14 bool    gDisableDeactivation = false;
15
16 float gLinearSleepingTreshold = 0.8f;
17 float gAngularSleepingTreshold = 1.0f;
18
19 #include "Dynamics/MassProps.h"
20
21 SimdVector3 startVel(0,0,0);//-10000);
22 CcdPhysicsController::CcdPhysicsController (const CcdConstructionInfo& ci)
23 {
24         m_collisionDelay = 0;
25         m_newClientInfo = 0;
26         
27         m_MotionState = ci.m_MotionState;
28
29
30         SimdTransform trans;
31         float tmp[3];
32         m_MotionState->getWorldPosition(tmp[0],tmp[1],tmp[2]);
33         trans.setOrigin(SimdVector3(tmp[0],tmp[1],tmp[2]));
34
35         SimdQuaternion orn;
36         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
37         trans.setRotation(orn);
38
39         MassProps mp(ci.m_mass, ci.m_localInertiaTensor);
40
41         m_body = new RigidBody(mp,0,0,ci.m_friction,ci.m_restitution);
42
43         m_body->SetCollisionShape( ci.m_collisionShape);
44
45         
46         m_broadphaseHandle = ci.m_broadphaseHandle;
47
48
49         //
50         // init the rigidbody properly
51         //
52         
53         m_body->setMassProps(ci.m_mass, ci.m_localInertiaTensor);
54         m_body->setGravity( ci.m_gravity);
55
56         
57         m_body->setDamping(ci.m_linearDamping, ci.m_angularDamping);
58
59
60         m_body->setCenterOfMassTransform( trans );
61
62         #ifdef WIN32
63         if (m_body->getInvMass())
64                 m_body->setLinearVelocity(startVel);
65         #endif
66
67 }
68
69 CcdPhysicsController::~CcdPhysicsController()
70 {
71         //will be reference counted, due to sharing
72         delete m_MotionState;
73         delete m_body;
74 }
75
76                 /**
77                         SynchronizeMotionStates ynchronizes dynas, kinematic and deformable entities (and do 'late binding')
78                 */
79 bool            CcdPhysicsController::SynchronizeMotionStates(float time)
80 {
81         const SimdVector3& worldPos = m_body->getCenterOfMassPosition();
82         m_MotionState->setWorldPosition(worldPos[0],worldPos[1],worldPos[2]);
83         
84         const SimdQuaternion& worldquat = m_body->getOrientation();
85         m_MotionState->setWorldOrientation(worldquat[0],worldquat[1],worldquat[2],worldquat[3]);
86
87         m_MotionState->calculateWorldTransformations();
88
89         float scale[3];
90         m_MotionState->getWorldScaling(scale[0],scale[1],scale[2]);
91         
92         SimdVector3 scaling(scale[0],scale[1],scale[2]);
93         m_body->GetCollisionShape()->setLocalScaling(scaling);
94
95         return true;
96 }
97
98 CollisionShape* CcdPhysicsController::GetCollisionShape() 
99
100         return m_body->GetCollisionShape();
101 }
102
103
104
105                 /**
106                         WriteMotionStateToDynamics synchronizes dynas, kinematic and deformable entities (and do 'late binding')
107                 */
108                 
109 void            CcdPhysicsController::WriteMotionStateToDynamics(bool nondynaonly)
110 {
111
112 }
113 void            CcdPhysicsController::WriteDynamicsToMotionState()
114 {
115 }
116                 // controller replication
117 void            CcdPhysicsController::PostProcessReplica(class PHY_IMotionState* motionstate,class PHY_IPhysicsController* parentctrl)
118 {
119 }
120
121                 // kinematic methods
122 void            CcdPhysicsController::RelativeTranslate(float dlocX,float dlocY,float dlocZ,bool local)
123 {
124         SimdTransform xform = m_body->getCenterOfMassTransform();
125         xform.setOrigin(xform.getOrigin() + SimdVector3(dlocX,dlocY,dlocZ));
126         this->m_body->setCenterOfMassTransform(xform);
127
128 }
129
130 void            CcdPhysicsController::RelativeRotate(const float rotval[9],bool local)
131 {
132         if (m_body )
133         {
134                 SimdMatrix3x3 drotmat(  rotval[0],rotval[1],rotval[2],
135                                                                 rotval[4],rotval[5],rotval[6],
136                                                                 rotval[8],rotval[9],rotval[10]);
137
138
139                 SimdMatrix3x3 currentOrn;
140                 GetWorldOrientation(currentOrn);
141
142                 SimdTransform xform = m_body->getCenterOfMassTransform();
143
144                 xform.setBasis(xform.getBasis()*(local ? 
145                 drotmat : (currentOrn.inverse() * drotmat * currentOrn)));
146
147                 m_body->setCenterOfMassTransform(xform);
148         }
149
150 }
151
152 void CcdPhysicsController::GetWorldOrientation(SimdMatrix3x3& mat)
153 {
154         float orn[4];
155         m_MotionState->getWorldOrientation(orn[0],orn[1],orn[2],orn[3]);
156         SimdQuaternion quat(orn[0],orn[1],orn[2],orn[3]);
157         mat.setRotation(quat);
158 }
159
160 void            CcdPhysicsController::getOrientation(float &quatImag0,float &quatImag1,float &quatImag2,float &quatReal)
161 {
162
163 }
164 void            CcdPhysicsController::setOrientation(float quatImag0,float quatImag1,float quatImag2,float quatReal)
165 {
166         m_body->activate();
167
168         SimdTransform xform  = m_body->getCenterOfMassTransform();
169         xform.setRotation(SimdQuaternion(quatImag0,quatImag1,quatImag2,quatReal));
170         m_body->setCenterOfMassTransform(xform);
171
172 }
173
174 void            CcdPhysicsController::setPosition(float posX,float posY,float posZ)
175 {
176         m_body->activate();
177
178         SimdTransform xform  = m_body->getCenterOfMassTransform();
179         xform.setOrigin(SimdVector3(posX,posY,posZ));
180         m_body->setCenterOfMassTransform(xform);
181
182 }
183 void            CcdPhysicsController::resolveCombinedVelocities(float linvelX,float linvelY,float linvelZ,float angVelX,float angVelY,float angVelZ)
184 {
185 }
186
187 void            CcdPhysicsController::getPosition(PHY__Vector3& pos) const
188 {
189         assert(0);
190 }
191
192 void            CcdPhysicsController::setScaling(float scaleX,float scaleY,float scaleZ)
193 {
194 }
195                 
196                 // physics methods
197 void            CcdPhysicsController::ApplyTorque(float torqueX,float torqueY,float torqueZ,bool local)
198 {
199 }
200 void            CcdPhysicsController::ApplyForce(float forceX,float forceY,float forceZ,bool local)
201 {
202 }
203 void            CcdPhysicsController::SetAngularVelocity(float ang_velX,float ang_velY,float ang_velZ,bool local)
204 {
205         SimdVector3 angvel(ang_velX,ang_velY,ang_velZ);
206
207         m_body->setAngularVelocity(angvel);
208
209 }
210 void            CcdPhysicsController::SetLinearVelocity(float lin_velX,float lin_velY,float lin_velZ,bool local)
211 {
212
213         SimdVector3 linVel(lin_velX,lin_velY,lin_velZ);
214         m_body->setLinearVelocity(linVel);
215 }
216 void            CcdPhysicsController::applyImpulse(float attachX,float attachY,float attachZ, float impulseX,float impulseY,float impulseZ)
217 {
218         
219         SimdVector3 impulse(impulseX,impulseY,impulseZ);
220         SimdVector3 pos(attachX,attachY,attachZ);
221
222         m_body->activate();
223
224         m_body->applyImpulse(impulse,pos);
225
226 }
227 void            CcdPhysicsController::SetActive(bool active)
228 {
229 }
230                 // reading out information from physics
231 void            CcdPhysicsController::GetLinearVelocity(float& linvX,float& linvY,float& linvZ)
232 {
233 }
234 void            CcdPhysicsController::GetVelocity(const float posX,const float posY,const float posZ,float& linvX,float& linvY,float& linvZ)
235 {
236 }
237 void            CcdPhysicsController::getReactionForce(float& forceX,float& forceY,float& forceZ)
238 {
239 }
240
241                 // dyna's that are rigidbody are free in orientation, dyna's with non-rigidbody are restricted 
242 void            CcdPhysicsController::setRigidBody(bool rigid)
243 {
244 }
245
246                 // clientinfo for raycasts for example
247 void*           CcdPhysicsController::getNewClientInfo()
248 {
249         return m_newClientInfo;
250 }
251 void            CcdPhysicsController::setNewClientInfo(void* clientinfo)
252 {
253         m_newClientInfo = clientinfo;
254 }
255
256
257 void    CcdPhysicsController::UpdateDeactivation(float timeStep)
258 {
259         if ( (m_body->GetActivationState() == 2))
260                 return;
261         
262
263         if ((m_body->getLinearVelocity().length2() < gLinearSleepingTreshold*gLinearSleepingTreshold) &&
264                 (m_body->getAngularVelocity().length2() < gAngularSleepingTreshold*gAngularSleepingTreshold))
265         {
266                 m_body->m_deactivationTime += timeStep;
267         } else
268         {
269                 m_body->m_deactivationTime=0.f;
270                 m_body->SetActivationState(0);
271         }
272
273 }
274
275 bool CcdPhysicsController::wantsSleeping()
276 {
277
278         //disable deactivation
279         if (gDisableDeactivation || (gDeactivationTime == 0.f))
280                 return false;
281         //2 == ISLAND_SLEEPING, 3 == WANTS_DEACTIVATION
282         if ( (m_body->GetActivationState() == 2) || (m_body->GetActivationState() == 3))
283                 return true;
284
285         if (m_body->m_deactivationTime> gDeactivationTime)
286         {
287                 return true;
288         }
289         return false;
290 }
291