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