1
0

ODECharacter.cs 49 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375
  1. /*
  2. * Copyright (c) Contributors, http://opensimulator.org/
  3. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions are met:
  7. * * Redistributions of source code must retain the above copyright
  8. * notice, this list of conditions and the following disclaimer.
  9. * * Redistributions in binary form must reproduce the above copyright
  10. * notice, this list of conditions and the following disclaimer in the
  11. * documentation and/or other materials provided with the distribution.
  12. * * Neither the name of the OpenSimulator Project nor the
  13. * names of its contributors may be used to endorse or promote products
  14. * derived from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  17. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  20. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. using System;
  28. using System.Collections.Generic;
  29. using System.Reflection;
  30. using OpenMetaverse;
  31. using Ode.NET;
  32. using OpenSim.Framework;
  33. using OpenSim.Region.Physics.Manager;
  34. using log4net;
  35. namespace OpenSim.Region.Physics.OdePlugin
  36. {
  37. /// <summary>
  38. /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
  39. /// </summary>
  40. public enum dParam : int
  41. {
  42. LowStop = 0,
  43. HiStop = 1,
  44. Vel = 2,
  45. FMax = 3,
  46. FudgeFactor = 4,
  47. Bounce = 5,
  48. CFM = 6,
  49. StopERP = 7,
  50. StopCFM = 8,
  51. LoStop2 = 256,
  52. HiStop2 = 257,
  53. Vel2 = 258,
  54. FMax2 = 259,
  55. StopERP2 = 7 + 256,
  56. StopCFM2 = 8 + 256,
  57. LoStop3 = 512,
  58. HiStop3 = 513,
  59. Vel3 = 514,
  60. FMax3 = 515,
  61. StopERP3 = 7 + 512,
  62. StopCFM3 = 8 + 512
  63. }
  64. public class OdeCharacter : PhysicsActor
  65. {
  66. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  67. private Vector3 _position;
  68. private d.Vector3 _zeroPosition;
  69. // private d.Matrix3 m_StandUpRotation;
  70. private bool _zeroFlag = false;
  71. private bool m_lastUpdateSent = false;
  72. private Vector3 _velocity;
  73. private Vector3 _target_velocity;
  74. private Vector3 _acceleration;
  75. private Vector3 m_rotationalVelocity;
  76. private float m_mass = 80f;
  77. public float m_density = 60f;
  78. private bool m_pidControllerActive = true;
  79. public float PID_D = 800.0f;
  80. public float PID_P = 900.0f;
  81. //private static float POSTURE_SERVO = 10000.0f;
  82. public float CAPSULE_RADIUS = 0.37f;
  83. public float CAPSULE_LENGTH = 2.140599f;
  84. public float m_tensor = 3800000f;
  85. public float heightFudgeFactor = 0.52f;
  86. public float walkDivisor = 1.3f;
  87. public float runDivisor = 0.8f;
  88. private bool flying = false;
  89. private bool m_iscolliding = false;
  90. private bool m_iscollidingGround = false;
  91. private bool m_wascolliding = false;
  92. private bool m_wascollidingGround = false;
  93. private bool m_iscollidingObj = false;
  94. private bool m_alwaysRun = false;
  95. private bool m_hackSentFall = false;
  96. private bool m_hackSentFly = false;
  97. private int m_requestedUpdateFrequency = 0;
  98. private Vector3 m_taintPosition = Vector3.Zero;
  99. public uint m_localID = 0;
  100. public bool m_returnCollisions = false;
  101. // taints and their non-tainted counterparts
  102. public bool m_isPhysical = false; // the current physical status
  103. public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing)
  104. public float MinimumGroundFlightOffset = 3f;
  105. private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes.
  106. private float m_tiltMagnitudeWhenProjectedOnXYPlane = 0.1131371f; // used to introduce a fixed tilt because a straight-up capsule falls through terrain, probably a bug in terrain collider
  107. private float m_buoyancy = 0f;
  108. // private CollisionLocker ode;
  109. private string m_name = String.Empty;
  110. private bool[] m_colliderarr = new bool[11];
  111. private bool[] m_colliderGroundarr = new bool[11];
  112. // Default we're a Character
  113. private CollisionCategories m_collisionCategories = (CollisionCategories.Character);
  114. // Default, Collide with Other Geometries, spaces, bodies and characters.
  115. private CollisionCategories m_collisionFlags = (CollisionCategories.Geom
  116. | CollisionCategories.Space
  117. | CollisionCategories.Body
  118. | CollisionCategories.Character
  119. | CollisionCategories.Land);
  120. public IntPtr Body = IntPtr.Zero;
  121. private OdeScene _parent_scene;
  122. public IntPtr Shell = IntPtr.Zero;
  123. public IntPtr Amotor = IntPtr.Zero;
  124. public d.Mass ShellMass;
  125. public bool collidelock = false;
  126. public int m_eventsubscription = 0;
  127. private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
  128. // unique UUID of this character object
  129. public UUID m_uuid;
  130. public bool bad = false;
  131. public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, CollisionLocker dode, Vector3 size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor)
  132. {
  133. m_uuid = UUID.Random();
  134. if (pos.IsFinite())
  135. {
  136. if (pos.Z > 9999999f)
  137. {
  138. pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
  139. }
  140. if (pos.Z < -90000f)
  141. {
  142. pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
  143. }
  144. _position = pos;
  145. m_taintPosition.X = pos.X;
  146. m_taintPosition.Y = pos.Y;
  147. m_taintPosition.Z = pos.Z;
  148. }
  149. else
  150. {
  151. _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f);
  152. m_taintPosition.X = _position.X;
  153. m_taintPosition.Y = _position.Y;
  154. m_taintPosition.Z = _position.Z;
  155. m_log.Warn("[PHYSICS]: Got NaN Position on Character Create");
  156. }
  157. _parent_scene = parent_scene;
  158. PID_D = pid_d;
  159. PID_P = pid_p;
  160. CAPSULE_RADIUS = capsule_radius;
  161. m_tensor = tensor;
  162. m_density = density;
  163. heightFudgeFactor = height_fudge_factor;
  164. walkDivisor = walk_divisor;
  165. runDivisor = rundivisor;
  166. // m_StandUpRotation =
  167. // new d.Matrix3(0.5f, 0.7071068f, 0.5f, -0.7071068f, 0f, 0.7071068f, 0.5f, -0.7071068f,
  168. // 0.5f);
  169. for (int i = 0; i < 11; i++)
  170. {
  171. m_colliderarr[i] = false;
  172. }
  173. CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f;
  174. //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
  175. m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH;
  176. m_isPhysical = false; // current status: no ODE information exists
  177. m_tainted_isPhysical = true; // new tainted status: need to create ODE information
  178. _parent_scene.AddPhysicsActorTaint(this);
  179. m_name = avName;
  180. }
  181. public override int PhysicsActorType
  182. {
  183. get { return (int) ActorTypes.Agent; }
  184. set { return; }
  185. }
  186. /// <summary>
  187. /// If this is set, the avatar will move faster
  188. /// </summary>
  189. public override bool SetAlwaysRun
  190. {
  191. get { return m_alwaysRun; }
  192. set { m_alwaysRun = value; }
  193. }
  194. public override uint LocalID
  195. {
  196. set { m_localID = value; }
  197. }
  198. public override bool Grabbed
  199. {
  200. set { return; }
  201. }
  202. public override bool Selected
  203. {
  204. set { return; }
  205. }
  206. public override float Buoyancy
  207. {
  208. get { return m_buoyancy; }
  209. set { m_buoyancy = value; }
  210. }
  211. public override bool FloatOnWater
  212. {
  213. set { return; }
  214. }
  215. public override bool IsPhysical
  216. {
  217. get { return false; }
  218. set { return; }
  219. }
  220. public override bool ThrottleUpdates
  221. {
  222. get { return false; }
  223. set { return; }
  224. }
  225. public override bool Flying
  226. {
  227. get { return flying; }
  228. set
  229. {
  230. flying = value;
  231. // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying);
  232. }
  233. }
  234. /// <summary>
  235. /// Returns if the avatar is colliding in general.
  236. /// This includes the ground and objects and avatar.
  237. /// </summary>
  238. public override bool IsColliding
  239. {
  240. get { return m_iscolliding; }
  241. set
  242. {
  243. int i;
  244. int truecount = 0;
  245. int falsecount = 0;
  246. if (m_colliderarr.Length >= 10)
  247. {
  248. for (i = 0; i < 10; i++)
  249. {
  250. m_colliderarr[i] = m_colliderarr[i + 1];
  251. }
  252. }
  253. m_colliderarr[10] = value;
  254. for (i = 0; i < 11; i++)
  255. {
  256. if (m_colliderarr[i])
  257. {
  258. truecount++;
  259. }
  260. else
  261. {
  262. falsecount++;
  263. }
  264. }
  265. // Equal truecounts and false counts means we're colliding with something.
  266. if (falsecount > 1.2*truecount)
  267. {
  268. m_iscolliding = false;
  269. }
  270. else
  271. {
  272. m_iscolliding = true;
  273. }
  274. if (m_wascolliding != m_iscolliding)
  275. {
  276. //base.SendCollisionUpdate(new CollisionEventUpdate());
  277. }
  278. m_wascolliding = m_iscolliding;
  279. }
  280. }
  281. /// <summary>
  282. /// Returns if an avatar is colliding with the ground
  283. /// </summary>
  284. public override bool CollidingGround
  285. {
  286. get { return m_iscollidingGround; }
  287. set
  288. {
  289. // Collisions against the ground are not really reliable
  290. // So, to get a consistant value we have to average the current result over time
  291. // Currently we use 1 second = 10 calls to this.
  292. int i;
  293. int truecount = 0;
  294. int falsecount = 0;
  295. if (m_colliderGroundarr.Length >= 10)
  296. {
  297. for (i = 0; i < 10; i++)
  298. {
  299. m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
  300. }
  301. }
  302. m_colliderGroundarr[10] = value;
  303. for (i = 0; i < 11; i++)
  304. {
  305. if (m_colliderGroundarr[i])
  306. {
  307. truecount++;
  308. }
  309. else
  310. {
  311. falsecount++;
  312. }
  313. }
  314. // Equal truecounts and false counts means we're colliding with something.
  315. if (falsecount > 1.2*truecount)
  316. {
  317. m_iscollidingGround = false;
  318. }
  319. else
  320. {
  321. m_iscollidingGround = true;
  322. }
  323. if (m_wascollidingGround != m_iscollidingGround)
  324. {
  325. //base.SendCollisionUpdate(new CollisionEventUpdate());
  326. }
  327. m_wascollidingGround = m_iscollidingGround;
  328. }
  329. }
  330. /// <summary>
  331. /// Returns if the avatar is colliding with an object
  332. /// </summary>
  333. public override bool CollidingObj
  334. {
  335. get { return m_iscollidingObj; }
  336. set
  337. {
  338. m_iscollidingObj = value;
  339. if (value)
  340. m_pidControllerActive = false;
  341. else
  342. m_pidControllerActive = true;
  343. }
  344. }
  345. /// <summary>
  346. /// turn the PID controller on or off.
  347. /// The PID Controller will turn on all by itself in many situations
  348. /// </summary>
  349. /// <param name="status"></param>
  350. public void SetPidStatus(bool status)
  351. {
  352. m_pidControllerActive = status;
  353. }
  354. public override bool Stopped
  355. {
  356. get { return _zeroFlag; }
  357. }
  358. /// <summary>
  359. /// This 'puts' an avatar somewhere in the physics space.
  360. /// Not really a good choice unless you 'know' it's a good
  361. /// spot otherwise you're likely to orbit the avatar.
  362. /// </summary>
  363. public override Vector3 Position
  364. {
  365. get { return _position; }
  366. set
  367. {
  368. if (Body == IntPtr.Zero || Shell == IntPtr.Zero)
  369. {
  370. if (value.IsFinite())
  371. {
  372. if (value.Z > 9999999f)
  373. {
  374. value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
  375. }
  376. if (value.Z < -90000f)
  377. {
  378. value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5;
  379. }
  380. _position.X = value.X;
  381. _position.Y = value.Y;
  382. _position.Z = value.Z;
  383. m_taintPosition.X = value.X;
  384. m_taintPosition.Y = value.Y;
  385. m_taintPosition.Z = value.Z;
  386. _parent_scene.AddPhysicsActorTaint(this);
  387. }
  388. else
  389. {
  390. m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character");
  391. }
  392. }
  393. }
  394. }
  395. public override Vector3 RotationalVelocity
  396. {
  397. get { return m_rotationalVelocity; }
  398. set { m_rotationalVelocity = value; }
  399. }
  400. /// <summary>
  401. /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight
  402. /// and use it to offset landings properly
  403. /// </summary>
  404. public override Vector3 Size
  405. {
  406. get { return new Vector3(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
  407. set
  408. {
  409. if (value.IsFinite())
  410. {
  411. m_pidControllerActive = true;
  412. Vector3 SetSize = value;
  413. m_tainted_CAPSULE_LENGTH = (SetSize.Z*1.15f) - CAPSULE_RADIUS*2.0f;
  414. //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
  415. Velocity = Vector3.Zero;
  416. _parent_scene.AddPhysicsActorTaint(this);
  417. }
  418. else
  419. {
  420. m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character");
  421. }
  422. }
  423. }
  424. private void AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3 movementVector)
  425. {
  426. movementVector.Z = 0f;
  427. float magnitude = (float)Math.Sqrt((double)(movementVector.X * movementVector.X + movementVector.Y * movementVector.Y));
  428. if (magnitude < 0.1f) return;
  429. // normalize the velocity vector
  430. float invMagnitude = 1.0f / magnitude;
  431. movementVector.X *= invMagnitude;
  432. movementVector.Y *= invMagnitude;
  433. // if we change the capsule heading too often, the capsule can fall down
  434. // therefore we snap movement vector to just 1 of 4 predefined directions (ne, nw, se, sw),
  435. // meaning only 4 possible capsule tilt orientations
  436. if (movementVector.X > 0)
  437. {
  438. // east
  439. if (movementVector.Y > 0)
  440. {
  441. // northeast
  442. movementVector.X = (float)Math.Sqrt(2.0);
  443. movementVector.Y = (float)Math.Sqrt(2.0);
  444. }
  445. else
  446. {
  447. // southeast
  448. movementVector.X = (float)Math.Sqrt(2.0);
  449. movementVector.Y = -(float)Math.Sqrt(2.0);
  450. }
  451. }
  452. else
  453. {
  454. // west
  455. if (movementVector.Y > 0)
  456. {
  457. // northwest
  458. movementVector.X = -(float)Math.Sqrt(2.0);
  459. movementVector.Y = (float)Math.Sqrt(2.0);
  460. }
  461. else
  462. {
  463. // southwest
  464. movementVector.X = -(float)Math.Sqrt(2.0);
  465. movementVector.Y = -(float)Math.Sqrt(2.0);
  466. }
  467. }
  468. // movementVector.Z is zero
  469. // calculate tilt components based on desired amount of tilt and current (snapped) heading.
  470. // the "-" sign is to force the tilt to be OPPOSITE the direction of movement.
  471. float xTiltComponent = -movementVector.X * m_tiltMagnitudeWhenProjectedOnXYPlane;
  472. float yTiltComponent = -movementVector.Y * m_tiltMagnitudeWhenProjectedOnXYPlane;
  473. //m_log.Debug("[PHYSICS] changing avatar tilt");
  474. d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, xTiltComponent);
  475. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, xTiltComponent); // must be same as lowstop, else a different, spurious tilt is introduced
  476. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, yTiltComponent);
  477. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, yTiltComponent); // same as lowstop
  478. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f);
  479. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
  480. }
  481. /// <summary>
  482. /// This creates the Avatar's physical Surrogate at the position supplied
  483. /// </summary>
  484. /// <param name="npositionX"></param>
  485. /// <param name="npositionY"></param>
  486. /// <param name="npositionZ"></param>
  487. // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access
  488. // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only
  489. // place that is safe to call this routine AvatarGeomAndBodyCreation.
  490. private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ, float tensor)
  491. {
  492. //CAPSULE_LENGTH = -5;
  493. //CAPSULE_RADIUS = -5;
  494. int dAMotorEuler = 1;
  495. _parent_scene.waitForSpaceUnlock(_parent_scene.space);
  496. if (CAPSULE_LENGTH <= 0)
  497. {
  498. m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
  499. CAPSULE_LENGTH = 0.01f;
  500. }
  501. if (CAPSULE_RADIUS <= 0)
  502. {
  503. m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!");
  504. CAPSULE_RADIUS = 0.01f;
  505. }
  506. Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
  507. d.GeomSetCategoryBits(Shell, (int)m_collisionCategories);
  508. d.GeomSetCollideBits(Shell, (int)m_collisionFlags);
  509. d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH);
  510. Body = d.BodyCreate(_parent_scene.world);
  511. d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
  512. _position.X = npositionX;
  513. _position.Y = npositionY;
  514. _position.Z = npositionZ;
  515. m_taintPosition.X = npositionX;
  516. m_taintPosition.Y = npositionY;
  517. m_taintPosition.Z = npositionZ;
  518. d.BodySetMass(Body, ref ShellMass);
  519. d.Matrix3 m_caprot;
  520. // 90 Stand up on the cap of the capped cyllinder
  521. if (_parent_scene.IsAvCapsuleTilted)
  522. {
  523. d.RFromAxisAndAngle(out m_caprot, 1, 0, 1, (float)(Math.PI / 2));
  524. }
  525. else
  526. {
  527. d.RFromAxisAndAngle(out m_caprot, 0, 0, 1, (float)(Math.PI / 2));
  528. }
  529. d.GeomSetRotation(Shell, ref m_caprot);
  530. d.BodySetRotation(Body, ref m_caprot);
  531. d.GeomSetBody(Shell, Body);
  532. // The purpose of the AMotor here is to keep the avatar's physical
  533. // surrogate from rotating while moving
  534. Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
  535. d.JointAttach(Amotor, Body, IntPtr.Zero);
  536. d.JointSetAMotorMode(Amotor, dAMotorEuler);
  537. d.JointSetAMotorNumAxes(Amotor, 3);
  538. d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0);
  539. d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0);
  540. d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1);
  541. d.JointSetAMotorAngle(Amotor, 0, 0);
  542. d.JointSetAMotorAngle(Amotor, 1, 0);
  543. d.JointSetAMotorAngle(Amotor, 2, 0);
  544. // These lowstops and high stops are effectively (no wiggle room)
  545. if (_parent_scene.IsAvCapsuleTilted)
  546. {
  547. d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.000000000001f);
  548. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.000000000001f);
  549. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.000000000001f);
  550. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.000000000001f);
  551. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.000000000001f);
  552. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.000000000001f);
  553. }
  554. else
  555. {
  556. #region Documentation of capsule motor LowStop and HighStop parameters
  557. // Intentionally introduce some tilt into the capsule by setting
  558. // the motor stops to small epsilon values. This small tilt prevents
  559. // the capsule from falling into the terrain; a straight-up capsule
  560. // (with -0..0 motor stops) falls into the terrain for reasons yet
  561. // to be comprehended in their entirety.
  562. #endregion
  563. AlignAvatarTiltWithCurrentDirectionOfMovement(Vector3.Zero);
  564. d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.08f);
  565. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
  566. d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.08f);
  567. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.08f); // must be same as lowstop, else a different, spurious tilt is introduced
  568. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); // same as lowstop
  569. d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.08f); // same as lowstop
  570. }
  571. // Fudge factor is 1f by default, we're setting it to 0. We don't want it to Fudge or the
  572. // capped cyllinder will fall over
  573. d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
  574. d.JointSetAMotorParam(Amotor, (int)dParam.FMax, tensor);
  575. //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
  576. //d.QfromR(
  577. //d.Matrix3 checkrotation = new d.Matrix3(0.7071068,0.5, -0.7071068,
  578. //
  579. //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
  580. //standupStraight();
  581. }
  582. //
  583. /// <summary>
  584. /// Uses the capped cyllinder volume formula to calculate the avatar's mass.
  585. /// This may be used in calculations in the scene/scenepresence
  586. /// </summary>
  587. public override float Mass
  588. {
  589. get
  590. {
  591. float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH);
  592. return m_density*AVvolume;
  593. }
  594. }
  595. public override void link(PhysicsActor obj)
  596. {
  597. }
  598. public override void delink()
  599. {
  600. }
  601. public override void LockAngularMotion(Vector3 axis)
  602. {
  603. }
  604. // This code is very useful. Written by DanX0r. We're just not using it right now.
  605. // Commented out to prevent a warning.
  606. //
  607. // private void standupStraight()
  608. // {
  609. // // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air.
  610. // // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you
  611. // // change appearance and when you enter the simulator
  612. // // After this routine is done, the amotor stabilizes much quicker
  613. // d.Vector3 feet;
  614. // d.Vector3 head;
  615. // d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
  616. // d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
  617. // float posture = head.Z - feet.Z;
  618. // // restoring force proportional to lack of posture:
  619. // float servo = (2.5f - posture) * POSTURE_SERVO;
  620. // d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
  621. // d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
  622. // //d.Matrix3 bodyrotation = d.BodyGetRotation(Body);
  623. // //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22);
  624. // }
  625. public override Vector3 Force
  626. {
  627. get { return _target_velocity; }
  628. set { return; }
  629. }
  630. public override int VehicleType
  631. {
  632. get { return 0; }
  633. set { return; }
  634. }
  635. public override void VehicleFloatParam(int param, float value)
  636. {
  637. }
  638. public override void VehicleVectorParam(int param, Vector3 value)
  639. {
  640. }
  641. public override void VehicleRotationParam(int param, Quaternion rotation)
  642. {
  643. }
  644. public override void VehicleFlags(int param, bool remove)
  645. {
  646. }
  647. public override void SetVolumeDetect(int param)
  648. {
  649. }
  650. public override Vector3 CenterOfMass
  651. {
  652. get { return Vector3.Zero; }
  653. }
  654. public override Vector3 GeometricCenter
  655. {
  656. get { return Vector3.Zero; }
  657. }
  658. public override PrimitiveBaseShape Shape
  659. {
  660. set { return; }
  661. }
  662. public override Vector3 Velocity
  663. {
  664. get {
  665. // There's a problem with Vector3.Zero! Don't Use it Here!
  666. if (_zeroFlag)
  667. return Vector3.Zero;
  668. m_lastUpdateSent = false;
  669. return _velocity;
  670. }
  671. set
  672. {
  673. if (value.IsFinite())
  674. {
  675. m_pidControllerActive = true;
  676. _target_velocity = value;
  677. }
  678. else
  679. {
  680. m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character");
  681. }
  682. }
  683. }
  684. public override Vector3 Torque
  685. {
  686. get { return Vector3.Zero; }
  687. set { return; }
  688. }
  689. public override float CollisionScore
  690. {
  691. get { return 0f; }
  692. set { }
  693. }
  694. public override bool Kinematic
  695. {
  696. get { return false; }
  697. set { }
  698. }
  699. public override Quaternion Orientation
  700. {
  701. get { return Quaternion.Identity; }
  702. set {
  703. //Matrix3 or = Orientation.ToRotationMatrix();
  704. //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22);
  705. //d.BodySetRotation(Body, ref ord);
  706. }
  707. }
  708. public override Vector3 Acceleration
  709. {
  710. get { return _acceleration; }
  711. }
  712. public void SetAcceleration(Vector3 accel)
  713. {
  714. m_pidControllerActive = true;
  715. _acceleration = accel;
  716. }
  717. /// <summary>
  718. /// Adds the force supplied to the Target Velocity
  719. /// The PID controller takes this target velocity and tries to make it a reality
  720. /// </summary>
  721. /// <param name="force"></param>
  722. public override void AddForce(Vector3 force, bool pushforce)
  723. {
  724. if (force.IsFinite())
  725. {
  726. if (pushforce)
  727. {
  728. m_pidControllerActive = false;
  729. force *= 100f;
  730. doForce(force);
  731. // If uncommented, things get pushed off world
  732. //
  733. // m_log.Debug("Push!");
  734. // _target_velocity.X += force.X;
  735. // _target_velocity.Y += force.Y;
  736. // _target_velocity.Z += force.Z;
  737. }
  738. else
  739. {
  740. m_pidControllerActive = true;
  741. _target_velocity.X += force.X;
  742. _target_velocity.Y += force.Y;
  743. _target_velocity.Z += force.Z;
  744. }
  745. }
  746. else
  747. {
  748. m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character");
  749. }
  750. //m_lastUpdateSent = false;
  751. }
  752. public override void AddAngularForce(Vector3 force, bool pushforce)
  753. {
  754. }
  755. /// <summary>
  756. /// After all of the forces add up with 'add force' we apply them with doForce
  757. /// </summary>
  758. /// <param name="force"></param>
  759. public void doForce(Vector3 force)
  760. {
  761. if (!collidelock)
  762. {
  763. d.BodyAddForce(Body, force.X, force.Y, force.Z);
  764. //d.BodySetRotation(Body, ref m_StandUpRotation);
  765. //standupStraight();
  766. }
  767. }
  768. public override void SetMomentum(Vector3 momentum)
  769. {
  770. }
  771. /// <summary>
  772. /// Called from Simulate
  773. /// This is the avatar's movement control + PID Controller
  774. /// </summary>
  775. /// <param name="timeStep"></param>
  776. public void Move(float timeStep, List<OdeCharacter> defects)
  777. {
  778. // no lock; for now it's only called from within Simulate()
  779. // If the PID Controller isn't active then we set our force
  780. // calculating base velocity to the current position
  781. if (Body == IntPtr.Zero)
  782. return;
  783. if (m_pidControllerActive == false)
  784. {
  785. _zeroPosition = d.BodyGetPosition(Body);
  786. }
  787. //PidStatus = true;
  788. d.Vector3 localpos = d.BodyGetPosition(Body);
  789. Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z);
  790. if (!localPos.IsFinite())
  791. {
  792. m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
  793. defects.Add(this);
  794. // _parent_scene.RemoveCharacter(this);
  795. // destroy avatar capsule and related ODE data
  796. if (Amotor != IntPtr.Zero)
  797. {
  798. // Kill the Amotor
  799. d.JointDestroy(Amotor);
  800. Amotor = IntPtr.Zero;
  801. }
  802. //kill the Geometry
  803. _parent_scene.waitForSpaceUnlock(_parent_scene.space);
  804. if (Body != IntPtr.Zero)
  805. {
  806. //kill the body
  807. d.BodyDestroy(Body);
  808. Body = IntPtr.Zero;
  809. }
  810. if (Shell != IntPtr.Zero)
  811. {
  812. d.GeomDestroy(Shell);
  813. _parent_scene.geom_name_map.Remove(Shell);
  814. Shell = IntPtr.Zero;
  815. }
  816. return;
  817. }
  818. Vector3 vec = Vector3.Zero;
  819. d.Vector3 vel = d.BodyGetLinearVel(Body);
  820. float movementdivisor = 1f;
  821. if (!m_alwaysRun)
  822. {
  823. movementdivisor = walkDivisor;
  824. }
  825. else
  826. {
  827. movementdivisor = runDivisor;
  828. }
  829. // if velocity is zero, use position control; otherwise, velocity control
  830. if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
  831. {
  832. // keep track of where we stopped. No more slippin' & slidin'
  833. if (!_zeroFlag)
  834. {
  835. _zeroFlag = true;
  836. _zeroPosition = d.BodyGetPosition(Body);
  837. }
  838. if (m_pidControllerActive)
  839. {
  840. // We only want to deactivate the PID Controller if we think we want to have our surrogate
  841. // react to the physics scene by moving it's position.
  842. // Avatar to Avatar collisions
  843. // Prim to avatar collisions
  844. d.Vector3 pos = d.BodyGetPosition(Body);
  845. vec.X = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
  846. vec.Y = (_target_velocity.Y - vel.Y)*(PID_D) + (_zeroPosition.Y - pos.Y)* (PID_P * 2);
  847. if (flying)
  848. {
  849. vec.Z = (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
  850. }
  851. }
  852. //PidStatus = true;
  853. }
  854. else
  855. {
  856. m_pidControllerActive = true;
  857. _zeroFlag = false;
  858. if (m_iscolliding && !flying)
  859. {
  860. // We're standing on something
  861. vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D);
  862. vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D);
  863. }
  864. else if (m_iscolliding && flying)
  865. {
  866. // We're flying and colliding with something
  867. vec.X = ((_target_velocity.X/movementdivisor) - vel.X)*(PID_D / 16);
  868. vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y)*(PID_D / 16);
  869. }
  870. else if (!m_iscolliding && flying)
  871. {
  872. // we're in mid air suspended
  873. vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * (PID_D/6);
  874. vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D/6);
  875. }
  876. if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
  877. {
  878. // We're colliding with something and we're not flying but we're moving
  879. // This means we're walking or running.
  880. d.Vector3 pos = d.BodyGetPosition(Body);
  881. vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P;
  882. if (_target_velocity.X > 0)
  883. {
  884. vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
  885. }
  886. if (_target_velocity.Y > 0)
  887. {
  888. vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
  889. }
  890. }
  891. else if (!m_iscolliding && !flying)
  892. {
  893. // we're not colliding and we're not flying so that means we're falling!
  894. // m_iscolliding includes collisions with the ground.
  895. // d.Vector3 pos = d.BodyGetPosition(Body);
  896. if (_target_velocity.X > 0)
  897. {
  898. vec.X = ((_target_velocity.X - vel.X)/1.2f)*PID_D;
  899. }
  900. if (_target_velocity.Y > 0)
  901. {
  902. vec.Y = ((_target_velocity.Y - vel.Y)/1.2f)*PID_D;
  903. }
  904. }
  905. if (flying)
  906. {
  907. vec.Z = (_target_velocity.Z - vel.Z) * (PID_D);
  908. }
  909. }
  910. if (flying)
  911. {
  912. vec.Z += ((-1 * _parent_scene.gravityz)*m_mass);
  913. //Added for auto fly height. Kitto Flora
  914. //d.Vector3 pos = d.BodyGetPosition(Body);
  915. float target_altitude = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y) + MinimumGroundFlightOffset;
  916. if (_position.Z < target_altitude)
  917. {
  918. vec.Z += (target_altitude - _position.Z) * PID_P * 5.0f;
  919. }
  920. // end add Kitto Flora
  921. }
  922. if (vec.IsFinite())
  923. {
  924. doForce(vec);
  925. if (!_zeroFlag)
  926. {
  927. AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
  928. }
  929. }
  930. else
  931. {
  932. m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
  933. m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
  934. defects.Add(this);
  935. // _parent_scene.RemoveCharacter(this);
  936. // destroy avatar capsule and related ODE data
  937. if (Amotor != IntPtr.Zero)
  938. {
  939. // Kill the Amotor
  940. d.JointDestroy(Amotor);
  941. Amotor = IntPtr.Zero;
  942. }
  943. //kill the Geometry
  944. _parent_scene.waitForSpaceUnlock(_parent_scene.space);
  945. if (Body != IntPtr.Zero)
  946. {
  947. //kill the body
  948. d.BodyDestroy(Body);
  949. Body = IntPtr.Zero;
  950. }
  951. if (Shell != IntPtr.Zero)
  952. {
  953. d.GeomDestroy(Shell);
  954. _parent_scene.geom_name_map.Remove(Shell);
  955. Shell = IntPtr.Zero;
  956. }
  957. }
  958. }
  959. /// <summary>
  960. /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence.
  961. /// </summary>
  962. public void UpdatePositionAndVelocity()
  963. {
  964. // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
  965. d.Vector3 vec;
  966. try
  967. {
  968. vec = d.BodyGetPosition(Body);
  969. }
  970. catch (NullReferenceException)
  971. {
  972. bad = true;
  973. _parent_scene.BadCharacter(this);
  974. vec = new d.Vector3(_position.X, _position.Y, _position.Z);
  975. base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem!
  976. m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid);
  977. }
  978. // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
  979. if (vec.X < 0.0f) vec.X = 0.0f;
  980. if (vec.Y < 0.0f) vec.Y = 0.0f;
  981. if (vec.X > (int)_parent_scene.WorldExtents.X - 0.05f) vec.X = (int)_parent_scene.WorldExtents.X - 0.05f;
  982. if (vec.Y > (int)_parent_scene.WorldExtents.Y - 0.05f) vec.Y = (int)_parent_scene.WorldExtents.Y - 0.05f;
  983. _position.X = vec.X;
  984. _position.Y = vec.Y;
  985. _position.Z = vec.Z;
  986. // I think we need to update the taintPosition too -- Diva 12/24/10
  987. m_taintPosition.X = vec.X;
  988. m_taintPosition.Y = vec.Y;
  989. m_taintPosition.Z = vec.Z;
  990. // Did we move last? = zeroflag
  991. // This helps keep us from sliding all over
  992. if (_zeroFlag)
  993. {
  994. _velocity.X = 0.0f;
  995. _velocity.Y = 0.0f;
  996. _velocity.Z = 0.0f;
  997. // Did we send out the 'stopped' message?
  998. if (!m_lastUpdateSent)
  999. {
  1000. m_lastUpdateSent = true;
  1001. //base.RequestPhysicsterseUpdate();
  1002. }
  1003. }
  1004. else
  1005. {
  1006. m_lastUpdateSent = false;
  1007. try
  1008. {
  1009. vec = d.BodyGetLinearVel(Body);
  1010. }
  1011. catch (NullReferenceException)
  1012. {
  1013. vec.X = _velocity.X;
  1014. vec.Y = _velocity.Y;
  1015. vec.Z = _velocity.Z;
  1016. }
  1017. _velocity.X = (vec.X);
  1018. _velocity.Y = (vec.Y);
  1019. _velocity.Z = (vec.Z);
  1020. if (_velocity.Z < -6 && !m_hackSentFall)
  1021. {
  1022. m_hackSentFall = true;
  1023. m_pidControllerActive = false;
  1024. }
  1025. else if (flying && !m_hackSentFly)
  1026. {
  1027. //m_hackSentFly = true;
  1028. //base.SendCollisionUpdate(new CollisionEventUpdate());
  1029. }
  1030. else
  1031. {
  1032. m_hackSentFly = false;
  1033. m_hackSentFall = false;
  1034. }
  1035. }
  1036. }
  1037. /// <summary>
  1038. /// Cleanup the things we use in the scene.
  1039. /// </summary>
  1040. public void Destroy()
  1041. {
  1042. m_tainted_isPhysical = false;
  1043. _parent_scene.AddPhysicsActorTaint(this);
  1044. }
  1045. public override void CrossingFailure()
  1046. {
  1047. }
  1048. public override Vector3 PIDTarget { set { return; } }
  1049. public override bool PIDActive { set { return; } }
  1050. public override float PIDTau { set { return; } }
  1051. public override float PIDHoverHeight { set { return; } }
  1052. public override bool PIDHoverActive { set { return; } }
  1053. public override PIDHoverType PIDHoverType { set { return; } }
  1054. public override float PIDHoverTau { set { return; } }
  1055. public override Quaternion APIDTarget{ set { return; } }
  1056. public override bool APIDActive{ set { return; } }
  1057. public override float APIDStrength{ set { return; } }
  1058. public override float APIDDamping{ set { return; } }
  1059. public override void SubscribeEvents(int ms)
  1060. {
  1061. m_requestedUpdateFrequency = ms;
  1062. m_eventsubscription = ms;
  1063. _parent_scene.AddCollisionEventReporting(this);
  1064. }
  1065. public override void UnSubscribeEvents()
  1066. {
  1067. _parent_scene.RemoveCollisionEventReporting(this);
  1068. m_requestedUpdateFrequency = 0;
  1069. m_eventsubscription = 0;
  1070. }
  1071. public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
  1072. {
  1073. if (m_eventsubscription > 0)
  1074. {
  1075. // m_log.DebugFormat(
  1076. // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact);
  1077. CollisionEventsThisFrame.addCollider(CollidedWith, contact);
  1078. }
  1079. }
  1080. public void SendCollisions()
  1081. {
  1082. if (m_eventsubscription > m_requestedUpdateFrequency)
  1083. {
  1084. if (CollisionEventsThisFrame != null)
  1085. {
  1086. base.SendCollisionUpdate(CollisionEventsThisFrame);
  1087. }
  1088. CollisionEventsThisFrame = new CollisionEventUpdate();
  1089. m_eventsubscription = 0;
  1090. }
  1091. }
  1092. public override bool SubscribedEvents()
  1093. {
  1094. if (m_eventsubscription > 0)
  1095. return true;
  1096. return false;
  1097. }
  1098. public void ProcessTaints(float timestep)
  1099. {
  1100. if (m_tainted_isPhysical != m_isPhysical)
  1101. {
  1102. if (m_tainted_isPhysical)
  1103. {
  1104. // Create avatar capsule and related ODE data
  1105. if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero && Amotor == IntPtr.Zero))
  1106. {
  1107. m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
  1108. + (Shell!=IntPtr.Zero ? "Shell ":"")
  1109. + (Body!=IntPtr.Zero ? "Body ":"")
  1110. + (Amotor!=IntPtr.Zero ? "Amotor ":""));
  1111. }
  1112. AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z, m_tensor);
  1113. _parent_scene.geom_name_map[Shell] = m_name;
  1114. _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
  1115. _parent_scene.AddCharacter(this);
  1116. }
  1117. else
  1118. {
  1119. _parent_scene.RemoveCharacter(this);
  1120. // destroy avatar capsule and related ODE data
  1121. if (Amotor != IntPtr.Zero)
  1122. {
  1123. // Kill the Amotor
  1124. d.JointDestroy(Amotor);
  1125. Amotor = IntPtr.Zero;
  1126. }
  1127. //kill the Geometry
  1128. _parent_scene.waitForSpaceUnlock(_parent_scene.space);
  1129. if (Body != IntPtr.Zero)
  1130. {
  1131. //kill the body
  1132. d.BodyDestroy(Body);
  1133. Body = IntPtr.Zero;
  1134. }
  1135. if (Shell != IntPtr.Zero)
  1136. {
  1137. d.GeomDestroy(Shell);
  1138. _parent_scene.geom_name_map.Remove(Shell);
  1139. Shell = IntPtr.Zero;
  1140. }
  1141. }
  1142. m_isPhysical = m_tainted_isPhysical;
  1143. }
  1144. if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
  1145. {
  1146. if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero)
  1147. {
  1148. m_pidControllerActive = true;
  1149. // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
  1150. d.JointDestroy(Amotor);
  1151. float prevCapsule = CAPSULE_LENGTH;
  1152. CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
  1153. //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
  1154. d.BodyDestroy(Body);
  1155. d.GeomDestroy(Shell);
  1156. AvatarGeomAndBodyCreation(_position.X, _position.Y,
  1157. _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2), m_tensor);
  1158. Velocity = Vector3.Zero;
  1159. _parent_scene.geom_name_map[Shell] = m_name;
  1160. _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
  1161. }
  1162. else
  1163. {
  1164. m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - "
  1165. + (Shell==IntPtr.Zero ? "Shell ":"")
  1166. + (Body==IntPtr.Zero ? "Body ":"")
  1167. + (Amotor==IntPtr.Zero ? "Amotor ":""));
  1168. }
  1169. }
  1170. if (!m_taintPosition.ApproxEquals(_position, 0.05f))
  1171. {
  1172. if (Body != IntPtr.Zero)
  1173. {
  1174. d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);
  1175. _position.X = m_taintPosition.X;
  1176. _position.Y = m_taintPosition.Y;
  1177. _position.Z = m_taintPosition.Z;
  1178. }
  1179. }
  1180. }
  1181. internal void AddCollisionFrameTime(int p)
  1182. {
  1183. // protect it from overflow crashing
  1184. if (m_eventsubscription + p >= int.MaxValue)
  1185. m_eventsubscription = 0;
  1186. m_eventsubscription += p;
  1187. }
  1188. }
  1189. }