ODECharacter.cs 52 KB

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