1  // 

2  // Lol Engine 

3  // 

4  // Copyright: (c) 20102012 Sam Hocevar <sam@hocevar.net> 

5  // (c) 20092012 Cédric Lecacheur <jordx@free.fr> 

6  // (c) 20092012 Benjamin Huet <huet.benjamin@gmail.com> 

7  // This program is free software; you can redistribute it and/or 

8  // modify it under the terms of the Do What The Fuck You Want To 

9  // Public License, Version 2, as published by Sam Hocevar. See 

10  // http://sam.zoy.org/projects/COPYING.WTFPL for more details. 

11  // 

12  

13  #if defined HAVE_CONFIG_H 

14  # include "config.h" 

15  #endif 

16  

17  #define USE_LOL_CTRLR_CHARAC 

18  

19  #ifdef HAVE_PHYS_USE_BULLET 

20  #include "core.h"


21  #include <stdio.h> 

22  #include "../Include/LolBtPhysicsIntegration.h" 

23  #include "../Include/LolPhysics.h" 

24  #include "../Include/EasyCharacterController.h" 

25  #include "../Include/BulletCharacterController.h" 

26  //#include "LinearMath/btIDebugDraw.h" 

27  //#include "BulletCollision/CollisionDispatch/btGhostObject.h" 

28  //#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" 

29  //#include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" 

30  //#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" 

31  //#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" 

32  //#include "LinearMath/btDefaultMotionState.h" 

33  #endif //HAVE_PHYS_USE_BULLET


34  

35  

36  namespace lol 

37  { 

38  

39  namespace phys 

40  { 

41  

42  #ifdef USE_LOL_CTRLR_CHARAC 

43  #ifdef HAVE_PHYS_USE_BULLET 

44  

45  //SweepCallback used for Swweep Tests. 

46  class ClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback 

47  { 

48  public: 

49  ClosestNotMeConvexResultCallback(btCollisionObject* NewMe, const vec3& NewUp, float MinSlopeDot) : 

50  btCollisionWorld::ClosestConvexResultCallback(LOL2BTU_VEC3(vec3(.0f)), LOL2BTU_VEC3(vec3(.0f))), 

51  m_me(NewMe), 

52  m_up(NewUp), 

53  m_min_slope_dot(MinSlopeDot) { } 

54  

55  virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& ConvexResult, bool NormalInWorld) 

56  { 

57  //We hit ourselves, FAIL 

58  if (ConvexResult.m_hitCollisionObject == m_me) 

59  return btScalar(1.f); 

60  

61  vec3 WorldHitNomal(.0f); 

62  if (NormalInWorld) 

63  WorldHitNomal = BT2LOL_VEC3(ConvexResult.m_hitNormalLocal); 

64  else //need to transform Normal into worldspace 

65  { 

66  btVector3 TmpWorldHitNormal = ConvexResult.m_hitCollisionObject>getWorldTransform().getBasis() * ConvexResult.m_hitNormalLocal; 

67  WorldHitNomal = BT2LOL_VEC3(TmpWorldHitNormal); 

68  } 

69  

70  float DotUp = dot(m_up, WorldHitNomal); 

71  //We hit below the accepted slope_dot, FAIL 

72  if (DotUp < m_min_slope_dot) 

73  return btScalar(1.f); 

74  

75  //Continue to next. 

76  return ClosestConvexResultCallback::addSingleResult(ConvexResult, NormalInWorld); 

77  } 

78  protected: 

79  btCollisionObject* m_me; 

80  const vec3 m_up; 

81  float m_min_slope_dot; 

82  }; 

83  

84  //When called, will try to remove Character controller from its collision. 

85  bool BulletKinematicCharacterController::RecoverFromPenetration(btCollisionWorld* CollisionWorld) 

86  { 

87  bool HasPenetration = false; 

88  

89  //Retrieve all pair with us colliding. 

90  CollisionWorld>getDispatcher()>dispatchAllCollisionPairs(m_ghost_object>getOverlappingPairCache(), CollisionWorld>getDispatchInfo(), CollisionWorld>getDispatcher()); 

91  m_current_position = BT2LOLU_VEC3(m_ghost_object>getWorldTransform().getOrigin()); 

92  

93  float MaxPen = .0f; 

94  for (int i = 0; i < m_ghost_object>getOverlappingPairCache()>getNumOverlappingPairs(); i++) 

95  { 

96  m_manifold_array.resize(0); 

97  

98  //this is the equivalent of the "Touch algorithm". Maybe refactor ? 

99  btBroadphasePair* CollisionPair = &m_ghost_object>getOverlappingPairCache()>getOverlappingPairArray()[i]; 

100  if (CollisionPair>m_algorithm) 

101  CollisionPair>m_algorithm>getAllContactManifolds(m_manifold_array); 

102  

103  for (int j = 0; j < m_manifold_array.size(); ++j) 

104  { 

105  btPersistentManifold* CurMfold = m_manifold_array[j]; 

106  //Normal direction differs if we're Body0 

107  float DirSign = CurMfold>getBody0() == m_ghost_object ? 1.f : 1.f; 

108  

109  for (int k = 0; k < CurMfold>getNumContacts(); k++) 

110  { 

111  const btManifoldPoint& MfPoint = CurMfold>getContactPoint(k); 

112  float Dist = MfPoint.getDistance(); 

113  if (Dist < .0f) 

114  { 

115  if (Dist < MaxPen) 

116  { 

117  MaxPen = Dist; 

118  m_touching_normal = BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign; 

119  } 

120  m_current_position += BT2LOL_VEC3(MfPoint.m_normalWorldOnB) * DirSign * Dist * .2f; 

121  HasPenetration = true; 

122  } 

123  } 

124  } 

125  } 

126  

127  btTransform GObjMx = m_ghost_object>getWorldTransform(); 

128  GObjMx.setOrigin(LOL2BTU_VEC3(m_current_position)); 

129  m_ghost_object>setWorldTransform(GObjMx); 

130  

131  return HasPenetration; 

132  } 

133  

134  //When the Controller hits a wall, we modify the target so the controller will MoveStep along the wall. 

135  void BulletKinematicCharacterController::UpdateTargetOnHit(const vec3& HitNormal, float TangentMag, float NormalMag) 

136  { 

137  vec3 Movedir = m_target_position  m_current_position; 

138  float MoveLength = (float)length(Movedir); 

139  

140  if (MoveLength > SIMD_EPSILON) 

141  { 

142  Movedir = normalize(Movedir); 

143  

144  vec3 ReflectDir = normalize(GetReflectedDir(Movedir, HitNormal)); 

145  vec3 ParallelDir = ProjectDirOnNorm(ReflectDir, HitNormal); 

146  vec3 PerpindicularDir = ProjectDirOnNormPerpindicular(ReflectDir, HitNormal); 

147  

148  m_target_position = m_current_position; 

149  

150  if (NormalMag != .0f) 

151  m_target_position += PerpindicularDir * NormalMag * MoveLength; 

152  } 

153  } 

154  

155  //Handles the StepUp : Currently taking into account Stair step & Jump. 

156  void BulletKinematicCharacterController::StepUp(btCollisionWorld* world) 

157  { 

158  // phase 1: up 

159  vec3 UpDir = GetUpAxisDirections()[m_up_axis]; 

160  btTransform SweepStart, SweepEnd; 

161  SweepStart.setIdentity(); 

162  SweepEnd.setIdentity(); 

163  

164  m_target_position = m_current_position + UpDir * (m_step_height + (m_vertical_offset > 0.f ? m_vertical_offset : 0.f)); 

165  

166  /* FIXME: Handle HasPenetration properly */ 

167  SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position + UpDir * (m_convex_shape>getMargin() + m_added_margin))); 

168  SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position)); 

169  

170  ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, UpDir, float(0.7071)); 

171  SweepCallback.m_collisionFilterGroup = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterGroup; 

172  SweepCallback.m_collisionFilterMask = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterMask; 

173  

174  if (m_do_gobject_sweep_test) 

175  m_ghost_object>convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, world>getDispatchInfo().m_allowedCcdPenetration); 

176  else 

177  world>convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback); 

178  

179  if (SweepCallback.hasHit()) 

180  { 

181  // Only modify the position if the hit was a slope and not a wall or ceiling. 

182  if(SweepCallback.m_hitNormalWorld.dot(LOL2BTU_VEC3(UpDir)) > .0f) 

183  { 

184  // we moved up only a Fraction of the step height 

185  m_current_step_offset = m_step_height * SweepCallback.m_closestHitFraction; 

186  btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3) 

187  InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction); 

188  m_current_position = BT2LOLU_VEC3(InterpolPos); 

189  } 

190  m_vertical_velocity = .0f; 

191  m_vertical_offset = .0f; 

192  } 

193  else 

194  { 

195  m_current_step_offset = m_step_height; 

196  m_current_position = m_target_position; 

197  } 

198  } 

199  

200  //Handles the actual Movement. It actually moves in the 3 dimensions, function name is confusing. 

201  void BulletKinematicCharacterController::StepForwardAndStrafe(btCollisionWorld* CollisionWorld, const vec3& MoveStep) 

202  { 

203  // phase 2: forward and strafe 

204  m_target_position = m_current_position + MoveStep; 

205  btTransform SweepStart, SweepEnd; 

206  SweepStart.setIdentity(); 

207  SweepEnd.setIdentity(); 

208  

209  float Fraction = 1.f; 

210  float SqDist = .0f; 

211  

212  if (m_touching_contact && dot(m_normalized_direction, m_touching_normal) > .0f) 

213  UpdateTargetOnHit(m_touching_normal); 

214  

215  //Let's loop on movement, until Movement fraction if below 0.01, which means we've reached our destination. 

216  //Or until we'tried 10 times. 

217  int MaxMoveLoop = 10; 

218  while (Fraction > .01f && MaxMoveLoop > 0) 

219  { 

220  SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position)); 

221  SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position)); 

222  vec3 SweepDirNeg(m_current_position  m_target_position); 

223  

224  ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, SweepDirNeg, .0f); 

225  SweepCallback.m_collisionFilterGroup = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterGroup; 

226  SweepCallback.m_collisionFilterMask = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterMask; 

227  

228  //The sweep test is done with an added margin, so we use it and then discard it 

229  float SavedMargin = m_convex_shape>getMargin(); 

230  m_convex_shape>setMargin(SavedMargin + m_added_margin); //Apply Added Margin 

231  if (m_do_gobject_sweep_test) 

232  m_ghost_object>convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld>getDispatchInfo().m_allowedCcdPenetration); 

233  else 

234  CollisionWorld>convexSweepTest (m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld>getDispatchInfo().m_allowedCcdPenetration); 

235  m_convex_shape>setMargin(SavedMargin); //Restore saved margin 

236  

237  Fraction = SweepCallback.m_closestHitFraction; 

238  

239  if (SweepCallback.hasHit()) 

240  { 

241  //We moved only a Fraction 

242  float HitDist = (float)length(BT2LOLU_VEC3(SweepCallback.m_hitPointWorld)  m_current_position); 

243  

244  UpdateTargetOnHit(BT2LOL_VEC3(SweepCallback.m_hitNormalWorld)); 

245  vec3 NewDir = m_target_position  m_current_position; 

246  SqDist = sqlength(NewDir); 

247  if (SqDist > SIMD_EPSILON) 

248  { 

249  NewDir = normalize(NewDir); 

250  //See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." 

251  if (dot(NewDir, m_normalized_direction) <= .0f) 

252  break; 

253  } 

254  else 

255  break; 

256  } 

257  else //We moved whole way 

258  m_current_position = m_target_position; 

259  } 

260  } 

261  

262  //Handles the Stepdown : We go back on the ground at the end of the MoveStep. 

263  void BulletKinematicCharacterController::StepDown(btCollisionWorld* CollisionWorld, float DeltaTime) 

264  { 

265  // phase 3: down 

266  vec3 UpDir = GetUpAxisDirections()[m_up_axis]; 

267  btTransform SweepStart, SweepEnd; 

268  SweepStart.setIdentity(); 

269  SweepEnd.setIdentity(); 

270  

271  float DownVel = (m_vertical_velocity < 0.f ? m_vertical_velocity : 0.f) * DeltaTime; 

272  if (DownVel > .0f && DownVel < m_step_height && (m_was_on_ground  !m_was_jumping)) 

273  DownVel = m_step_height; 

274  

275  vec3 StepDrop = UpDir * (m_current_step_offset + DownVel); 

276  m_target_position = StepDrop; 

277  

278  SweepStart.setOrigin(LOL2BTU_VEC3(m_current_position)); 

279  SweepEnd.setOrigin(LOL2BTU_VEC3(m_target_position)); 

280  

281  ClosestNotMeConvexResultCallback SweepCallback(m_ghost_object, UpDir, m_max_slope_cosine); 

282  SweepCallback.m_collisionFilterGroup = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterGroup; 

283  SweepCallback.m_collisionFilterMask = GetGhostObject()>getBroadphaseHandle()>m_collisionFilterMask; 

284  

285  if (m_do_gobject_sweep_test) 

286  m_ghost_object>convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld>getDispatchInfo().m_allowedCcdPenetration); 

287  else 

288  CollisionWorld>convexSweepTest(m_convex_shape, SweepStart, SweepEnd, SweepCallback, CollisionWorld>getDispatchInfo().m_allowedCcdPenetration); 

289  

290  if (SweepCallback.hasHit()) 

291  { 

292  // we dropped a Fraction of the height > hit floor 

293  btVector3 InterpolPos; //TODO : REPLACE BY INTERPOLATE3/LERP(VEC3) 

294  InterpolPos.setInterpolate3(LOL2BTU_VEC3(m_current_position), LOL2BTU_VEC3(m_target_position), SweepCallback.m_closestHitFraction); 

295  m_current_position = BT2LOLU_VEC3(InterpolPos); 

296  m_vertical_velocity = .0f; 

297  m_vertical_offset = .0f; 

298  m_was_jumping = false; 

299  } 

300  else // we dropped the full height 

301  m_current_position = m_target_position; 

302  } 

303  

304  //The PreStepis done in order to recover from any HasPenetration. 

305  void BulletKinematicCharacterController::PreStep(btCollisionWorld* CollisionWorld) 

306  { 

307  int MaxPenetrationLoop = 0; 

308  m_touching_contact = false; 

309  

310  while (RecoverFromPenetration(CollisionWorld)) 

311  { 

312  MaxPenetrationLoop++; 

313  m_touching_contact = true; 

314  if (MaxPenetrationLoop > 4) 

315  break; 

316  } 

317  

318  m_current_position = BT2LOLU_VEC3(m_ghost_object>getWorldTransform().getOrigin()); 

319  m_target_position = m_current_position; 

320  } 

321  

322  //And so we step : 

323  //StepUpfirst, then movement, then StepDownon the ground. 

324  void BulletKinematicCharacterController::PlayerStep(btCollisionWorld* CollisionWorld, float DeltaTime) 

325  { 

326  // quick check... 

327  if (!m_use_walk_direction && m_velocity_time_interval <= .0f) 

328  return; // no motion 

329  

330  m_was_on_ground = OnGround(); 

331  

332  // Update fall velocity. 

333  m_vertical_velocity = m_gravity * DeltaTime; 

334  if(m_vertical_velocity > .0f && m_vertical_velocity > m_jump_speed) 

335  m_vertical_velocity = m_jump_speed; 

336  

337  if(m_vertical_velocity < .0f && btFabs(m_vertical_velocity) > btFabs(m_fall_speed)) 

338  m_vertical_velocity = btFabs(m_fall_speed); 

339  m_vertical_offset = m_vertical_velocity * DeltaTime; 

340  

341  btTransform NewTransform; 

342  NewTransform = m_ghost_object>getWorldTransform(); 

343  

344  vec3 MoveStep(.0f); 

345  if (m_use_walk_direction) 

346  MoveStep = m_walk_direction; 

347  else 

348  { 

349  //Still have some time left for moving! 

350  float dtMoving = (DeltaTime < m_velocity_time_interval) ? DeltaTime : m_velocity_time_interval; 

351  m_velocity_time_interval = DeltaTime; 

352  

353  // how far will we MoveStep while we are moving? 

354  MoveStep = m_walk_direction * dtMoving; 

355  } 

356  

357  //Okay, step ! 

358  StepUp(CollisionWorld); 

359  StepForwardAndStrafe(CollisionWorld, MoveStep); 

360  StepDown(CollisionWorld, DeltaTime); 

361  

362  //Movement finished, update World transform 

363  NewTransform.setOrigin(LOL2BTU_VEC3(m_current_position)); 

364  m_ghost_object>setWorldTransform(NewTransform); 

365  } 

366  

367  //should MoveStep Jump logic in EasyCC 

368  void BulletKinematicCharacterController::Jump() 

369  { 

370  if (!CanJump()) 

371  return; 

372  

373  m_vertical_velocity = m_jump_speed; 

374  m_was_jumping = true; 

375  } 

376  

377  #endif // HAVE_PHYS_USE_BULLET 

378  #endif // USE_LOL_CTRLR_CHARAC 

379  

380  } /* namespace phys */ 

381  

382  } /* namespace lol */ 
