ODECharacter.cs 52 KB

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