b2Body.cpp
上传用户:gb3593
上传日期:2022-01-07
资源大小:3028k
文件大小:10k
源码类别:

游戏引擎

开发平台:

Visual C++

  1. /*
  2. * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
  3. *
  4. * This software is provided 'as-is', without any express or implied
  5. * warranty.  In no event will the authors be held liable for any damages
  6. * arising from the use of this software.
  7. * Permission is granted to anyone to use this software for any purpose,
  8. * including commercial applications, and to alter it and redistribute it
  9. * freely, subject to the following restrictions:
  10. * 1. The origin of this software must not be misrepresented; you must not
  11. * claim that you wrote the original software. If you use this software
  12. * in a product, an acknowledgment in the product documentation would be
  13. * appreciated but is not required.
  14. * 2. Altered source versions must be plainly marked as such, and must not be
  15. * misrepresented as being the original software.
  16. * 3. This notice may not be removed or altered from any source distribution.
  17. */
  18. #include <Box2D/Dynamics/b2Body.h>
  19. #include <Box2D/Dynamics/b2Fixture.h>
  20. #include <Box2D/Dynamics/b2World.h>
  21. #include <Box2D/Dynamics/Contacts/b2Contact.h>
  22. #include <Box2D/Dynamics/Joints/b2Joint.h>
  23. b2Body::b2Body(const b2BodyDef* bd, b2World* world)
  24. {
  25. b2Assert(bd->position.IsValid());
  26. b2Assert(bd->linearVelocity.IsValid());
  27. b2Assert(b2IsValid(bd->angle));
  28. b2Assert(b2IsValid(bd->angularVelocity));
  29. b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
  30. b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
  31. b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
  32. m_flags = 0;
  33. if (bd->bullet)
  34. {
  35. m_flags |= e_bulletFlag;
  36. }
  37. if (bd->fixedRotation)
  38. {
  39. m_flags |= e_fixedRotationFlag;
  40. }
  41. if (bd->allowSleep)
  42. {
  43. m_flags |= e_autoSleepFlag;
  44. }
  45. if (bd->awake)
  46. {
  47. m_flags |= e_awakeFlag;
  48. }
  49. if (bd->active)
  50. {
  51. m_flags |= e_activeFlag;
  52. }
  53. m_world = world;
  54. m_xf.position = bd->position;
  55. m_xf.R.Set(bd->angle);
  56. m_sweep.localCenter.SetZero();
  57. m_sweep.a0 = m_sweep.a = bd->angle;
  58. m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  59. m_jointList = NULL;
  60. m_contactList = NULL;
  61. m_prev = NULL;
  62. m_next = NULL;
  63. m_linearVelocity = bd->linearVelocity;
  64. m_angularVelocity = bd->angularVelocity;
  65. m_linearDamping = bd->linearDamping;
  66. m_angularDamping = bd->angularDamping;
  67. m_force.SetZero();
  68. m_torque = 0.0f;
  69. m_sleepTime = 0.0f;
  70. m_type = bd->type;
  71. if (m_type == b2_dynamicBody)
  72. {
  73. m_mass = 1.0f;
  74. m_invMass = 1.0f;
  75. }
  76. else
  77. {
  78. m_mass = 0.0f;
  79. m_invMass = 0.0f;
  80. }
  81. m_I = 0.0f;
  82. m_invI = 0.0f;
  83. m_userData = bd->userData;
  84. m_fixtureList = NULL;
  85. m_fixtureCount = 0;
  86. }
  87. b2Body::~b2Body()
  88. {
  89. // shapes and joints are destroyed in b2World::Destroy
  90. }
  91. void b2Body::SetType(b2BodyType type)
  92. {
  93. if (m_type == type)
  94. {
  95. return;
  96. }
  97. m_type = type;
  98. ResetMassData();
  99. if (m_type == b2_staticBody)
  100. {
  101. m_linearVelocity.SetZero();
  102. m_angularVelocity = 0.0f;
  103. }
  104. SetAwake(true);
  105. m_force.SetZero();
  106. m_torque = 0.0f;
  107. // Since the body type changed, we need to flag contacts for filtering.
  108. for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next)
  109. {
  110. ce->contact->FlagForFiltering();
  111. }
  112. }
  113. b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
  114. {
  115. b2Assert(m_world->IsLocked() == false);
  116. if (m_world->IsLocked() == true)
  117. {
  118. return NULL;
  119. }
  120. b2BlockAllocator* allocator = &m_world->m_blockAllocator;
  121. void* memory = allocator->Allocate(sizeof(b2Fixture));
  122. b2Fixture* fixture = new (memory) b2Fixture;
  123. fixture->Create(allocator, this, def);
  124. if (m_flags & e_activeFlag)
  125. {
  126. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  127. fixture->CreateProxy(broadPhase, m_xf);
  128. }
  129. fixture->m_next = m_fixtureList;
  130. m_fixtureList = fixture;
  131. ++m_fixtureCount;
  132. fixture->m_body = this;
  133. // Adjust mass properties if needed.
  134. if (fixture->m_density > 0.0f)
  135. {
  136. ResetMassData();
  137. }
  138. // Let the world know we have a new fixture. This will cause new contacts
  139. // to be created at the beginning of the next time step.
  140. m_world->m_flags |= b2World::e_newFixture;
  141. return fixture;
  142. }
  143. b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
  144. {
  145. b2FixtureDef def;
  146. def.shape = shape;
  147. def.density = density;
  148. return CreateFixture(&def);
  149. }
  150. void b2Body::DestroyFixture(b2Fixture* fixture)
  151. {
  152. b2Assert(m_world->IsLocked() == false);
  153. if (m_world->IsLocked() == true)
  154. {
  155. return;
  156. }
  157. b2Assert(fixture->m_body == this);
  158. // Remove the fixture from this body's singly linked list.
  159. b2Assert(m_fixtureCount > 0);
  160. b2Fixture** node = &m_fixtureList;
  161. bool found = false;
  162. while (*node != NULL)
  163. {
  164. if (*node == fixture)
  165. {
  166. *node = fixture->m_next;
  167. found = true;
  168. break;
  169. }
  170. node = &(*node)->m_next;
  171. }
  172. // You tried to remove a shape that is not attached to this body.
  173. b2Assert(found);
  174. // Destroy any contacts associated with the fixture.
  175. b2ContactEdge* edge = m_contactList;
  176. while (edge)
  177. {
  178. b2Contact* c = edge->contact;
  179. edge = edge->next;
  180. b2Fixture* fixtureA = c->GetFixtureA();
  181. b2Fixture* fixtureB = c->GetFixtureB();
  182. if (fixture == fixtureA || fixture == fixtureB)
  183. {
  184. // This destroys the contact and removes it from
  185. // this body's contact list.
  186. m_world->m_contactManager.Destroy(c);
  187. }
  188. }
  189. b2BlockAllocator* allocator = &m_world->m_blockAllocator;
  190. if (m_flags & e_activeFlag)
  191. {
  192. b2Assert(fixture->m_proxyId != b2BroadPhase::e_nullProxy);
  193. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  194. fixture->DestroyProxy(broadPhase);
  195. }
  196. else
  197. {
  198. b2Assert(fixture->m_proxyId == b2BroadPhase::e_nullProxy);
  199. }
  200. fixture->Destroy(allocator);
  201. fixture->m_body = NULL;
  202. fixture->m_next = NULL;
  203. fixture->~b2Fixture();
  204. allocator->Free(fixture, sizeof(b2Fixture));
  205. --m_fixtureCount;
  206. // Reset the mass data.
  207. ResetMassData();
  208. }
  209. void b2Body::ResetMassData()
  210. {
  211. // Compute mass data from shapes. Each shape has its own density.
  212. m_mass = 0.0f;
  213. m_invMass = 0.0f;
  214. m_I = 0.0f;
  215. m_invI = 0.0f;
  216. m_sweep.localCenter.SetZero();
  217. // Static and kinematic bodies have zero mass.
  218. if (m_type == b2_staticBody || m_type == b2_kinematicBody)
  219. {
  220. m_sweep.c0 = m_sweep.c = m_xf.position;
  221. return;
  222. }
  223. b2Assert(m_type == b2_dynamicBody);
  224. // Accumulate mass over all fixtures.
  225. b2Vec2 center = b2Vec2_zero;
  226. for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
  227. {
  228. if (f->m_density == 0.0f)
  229. {
  230. continue;
  231. }
  232. b2MassData massData;
  233. f->GetMassData(&massData);
  234. m_mass += massData.mass;
  235. center += massData.mass * massData.center;
  236. m_I += massData.I;
  237. }
  238. // Compute center of mass.
  239. if (m_mass > 0.0f)
  240. {
  241. m_invMass = 1.0f / m_mass;
  242. center *= m_invMass;
  243. }
  244. else
  245. {
  246. // Force all dynamic bodies to have a positive mass.
  247. m_mass = 1.0f;
  248. m_invMass = 1.0f;
  249. }
  250. if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
  251. {
  252. // Center the inertia about the center of mass.
  253. m_I -= m_mass * b2Dot(center, center);
  254. b2Assert(m_I > 0.0f);
  255. m_invI = 1.0f / m_I;
  256. }
  257. else
  258. {
  259. m_I = 0.0f;
  260. m_invI = 0.0f;
  261. }
  262. // Move center of mass.
  263. b2Vec2 oldCenter = m_sweep.c;
  264. m_sweep.localCenter = center;
  265. m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  266. // Update center of mass velocity.
  267. m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
  268. }
  269. void b2Body::SetMassData(const b2MassData* massData)
  270. {
  271. b2Assert(m_world->IsLocked() == false);
  272. if (m_world->IsLocked() == true)
  273. {
  274. return;
  275. }
  276. if (m_type != b2_dynamicBody)
  277. {
  278. return;
  279. }
  280. m_invMass = 0.0f;
  281. m_I = 0.0f;
  282. m_invI = 0.0f;
  283. m_mass = massData->mass;
  284. if (m_mass <= 0.0f)
  285. {
  286. m_mass = 1.0f;
  287. }
  288. m_invMass = 1.0f / m_mass;
  289. if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
  290. {
  291. m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
  292. b2Assert(m_I > 0.0f);
  293. m_invI = 1.0f / m_I;
  294. }
  295. // Move center of mass.
  296. b2Vec2 oldCenter = m_sweep.c;
  297. m_sweep.localCenter = massData->center;
  298. m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  299. // Update center of mass velocity.
  300. m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
  301. }
  302. bool b2Body::ShouldCollide(const b2Body* other) const
  303. {
  304. // At least one body should be dynamic.
  305. if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
  306. {
  307. return false;
  308. }
  309. // Does a joint prevent collision?
  310. for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
  311. {
  312. if (jn->other == other)
  313. {
  314. if (jn->joint->m_collideConnected == false)
  315. {
  316. return false;
  317. }
  318. }
  319. }
  320. return true;
  321. }
  322. void b2Body::SetTransform(const b2Vec2& position, float32 angle)
  323. {
  324. b2Assert(m_world->IsLocked() == false);
  325. if (m_world->IsLocked() == true)
  326. {
  327. return;
  328. }
  329. m_xf.R.Set(angle);
  330. m_xf.position = position;
  331. m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  332. m_sweep.a0 = m_sweep.a = angle;
  333. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  334. for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
  335. {
  336. f->Synchronize(broadPhase, m_xf, m_xf);
  337. }
  338. m_world->m_contactManager.FindNewContacts();
  339. }
  340. void b2Body::SynchronizeFixtures()
  341. {
  342. b2Transform xf1;
  343. xf1.R.Set(m_sweep.a0);
  344. xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
  345. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  346. for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
  347. {
  348. f->Synchronize(broadPhase, xf1, m_xf);
  349. }
  350. }
  351. void b2Body::SetActive(bool flag)
  352. {
  353. if (flag == IsActive())
  354. {
  355. return;
  356. }
  357. if (flag)
  358. {
  359. m_flags |= e_activeFlag;
  360. // Create all proxies.
  361. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  362. for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
  363. {
  364. f->CreateProxy(broadPhase, m_xf);
  365. }
  366. // Contacts are created the next time step.
  367. }
  368. else
  369. {
  370. m_flags &= ~e_activeFlag;
  371. // Destroy all proxies.
  372. b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
  373. for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
  374. {
  375. f->DestroyProxy(broadPhase);
  376. }
  377. // Destroy the attached contacts.
  378. b2ContactEdge* ce = m_contactList;
  379. while (ce)
  380. {
  381. b2ContactEdge* ce0 = ce;
  382. ce = ce->next;
  383. m_world->m_contactManager.Destroy(ce0->contact);
  384. }
  385. m_contactList = NULL;
  386. }
  387. }