BSActorAvatarMove.cs 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409
  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 copyrightD
  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.Linq;
  30. using System.Text;
  31. using OpenSim.Region.Physics.Manager;
  32. using OMV = OpenMetaverse;
  33. namespace OpenSim.Region.Physics.BulletSPlugin
  34. {
  35. public class BSActorAvatarMove : BSActor
  36. {
  37. BSVMotor m_velocityMotor;
  38. // Set to true if we think we're going up stairs.
  39. // This state is remembered because collisions will turn on and off as we go up stairs.
  40. int m_walkingUpStairs;
  41. // The amount the step up is applying. Used to smooth stair walking.
  42. float m_lastStepUp;
  43. // Jumping happens over several frames. If use applies up force while colliding, start the
  44. // jump and allow the jump to continue for this number of frames.
  45. int m_jumpFrames = 0;
  46. float m_jumpVelocity = 0f;
  47. public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
  48. : base(physicsScene, pObj, actorName)
  49. {
  50. m_velocityMotor = null;
  51. m_walkingUpStairs = 0;
  52. m_physicsScene.DetailLog("{0},BSActorAvatarMove,constructor", m_controllingPrim.LocalID);
  53. }
  54. // BSActor.isActive
  55. public override bool isActive
  56. {
  57. get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
  58. }
  59. // Release any connections and resources used by the actor.
  60. // BSActor.Dispose()
  61. public override void Dispose()
  62. {
  63. base.SetEnabled(false);
  64. // Now that turned off, remove any state we have in the scene.
  65. Refresh();
  66. }
  67. // Called when physical parameters (properties set in Bullet) need to be re-applied.
  68. // Called at taint-time.
  69. // BSActor.Refresh()
  70. public override void Refresh()
  71. {
  72. m_physicsScene.DetailLog("{0},BSActorAvatarMove,refresh", m_controllingPrim.LocalID);
  73. // If the object is physically active, add the hoverer prestep action
  74. if (isActive)
  75. {
  76. ActivateAvatarMove();
  77. }
  78. else
  79. {
  80. DeactivateAvatarMove();
  81. }
  82. }
  83. // The object's physical representation is being rebuilt so pick up any physical dependencies (constraints, ...).
  84. // Register a prestep action to restore physical requirements before the next simulation step.
  85. // Called at taint-time.
  86. // BSActor.RemoveDependencies()
  87. public override void RemoveDependencies()
  88. {
  89. // Nothing to do for the hoverer since it is all software at pre-step action time.
  90. }
  91. // Usually called when target velocity changes to set the current velocity and the target
  92. // into the movement motor.
  93. public void SetVelocityAndTarget(OMV.Vector3 vel, OMV.Vector3 targ, bool inTaintTime)
  94. {
  95. m_physicsScene.TaintedObject(inTaintTime, "BSActorAvatarMove.setVelocityAndTarget", delegate()
  96. {
  97. if (m_velocityMotor != null)
  98. {
  99. m_velocityMotor.Reset();
  100. m_velocityMotor.SetTarget(targ);
  101. m_velocityMotor.SetCurrent(vel);
  102. m_velocityMotor.Enabled = true;
  103. }
  104. });
  105. }
  106. // If a hover motor has not been created, create one and start the hovering.
  107. private void ActivateAvatarMove()
  108. {
  109. if (m_velocityMotor == null)
  110. {
  111. // Infinite decay and timescale values so motor only changes current to target values.
  112. m_velocityMotor = new BSVMotor("BSCharacter.Velocity",
  113. 0.2f, // time scale
  114. BSMotor.Infinite, // decay time scale
  115. 1f // efficiency
  116. );
  117. // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
  118. SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);
  119. m_physicsScene.BeforeStep += Mover;
  120. m_controllingPrim.OnPreUpdateProperty += Process_OnPreUpdateProperty;
  121. m_walkingUpStairs = 0;
  122. }
  123. }
  124. private void DeactivateAvatarMove()
  125. {
  126. if (m_velocityMotor != null)
  127. {
  128. m_controllingPrim.OnPreUpdateProperty -= Process_OnPreUpdateProperty;
  129. m_physicsScene.BeforeStep -= Mover;
  130. m_velocityMotor = null;
  131. }
  132. }
  133. // Called just before the simulation step. Update the vertical position for hoverness.
  134. private void Mover(float timeStep)
  135. {
  136. // Don't do movement while the object is selected.
  137. if (!isActive)
  138. return;
  139. // TODO: Decide if the step parameters should be changed depending on the avatar's
  140. // state (flying, colliding, ...). There is code in ODE to do this.
  141. // COMMENTARY: when the user is making the avatar walk, except for falling, the velocity
  142. // specified for the avatar is the one that should be used. For falling, if the avatar
  143. // is not flying and is not colliding then it is presumed to be falling and the Z
  144. // component is not fooled with (thus allowing gravity to do its thing).
  145. // When the avatar is standing, though, the user has specified a velocity of zero and
  146. // the avatar should be standing. But if the avatar is pushed by something in the world
  147. // (raising elevator platform, moving vehicle, ...) the avatar should be allowed to
  148. // move. Thus, the velocity cannot be forced to zero. The problem is that small velocity
  149. // errors can creap in and the avatar will slowly float off in some direction.
  150. // So, the problem is that, when an avatar is standing, we cannot tell creaping error
  151. // from real pushing.
  152. // The code below uses whether the collider is static or moving to decide whether to zero motion.
  153. m_velocityMotor.Step(timeStep);
  154. m_controllingPrim.IsStationary = false;
  155. // If we're not supposed to be moving, make sure things are zero.
  156. if (m_velocityMotor.ErrorIsZero() && m_velocityMotor.TargetValue == OMV.Vector3.Zero)
  157. {
  158. // The avatar shouldn't be moving
  159. m_velocityMotor.Zero();
  160. if (m_controllingPrim.IsColliding)
  161. {
  162. // If we are colliding with a stationary object, presume we're standing and don't move around
  163. if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.ColliderIsVolumeDetect)
  164. {
  165. m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
  166. m_controllingPrim.IsStationary = true;
  167. m_controllingPrim.ZeroMotion(true /* inTaintTime */);
  168. }
  169. // Standing has more friction on the ground
  170. if (m_controllingPrim.Friction != BSParam.AvatarStandingFriction)
  171. {
  172. m_controllingPrim.Friction = BSParam.AvatarStandingFriction;
  173. m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
  174. }
  175. }
  176. else
  177. {
  178. if (m_controllingPrim.Flying)
  179. {
  180. // Flying and not colliding and velocity nearly zero.
  181. m_controllingPrim.ZeroMotion(true /* inTaintTime */);
  182. }
  183. }
  184. m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}",
  185. m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding);
  186. }
  187. else
  188. {
  189. // Supposed to be moving.
  190. OMV.Vector3 stepVelocity = m_velocityMotor.CurrentValue;
  191. if (m_controllingPrim.Friction != BSParam.AvatarFriction)
  192. {
  193. // Probably starting to walk. Set friction to moving friction.
  194. m_controllingPrim.Friction = BSParam.AvatarFriction;
  195. m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
  196. }
  197. if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
  198. {
  199. stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
  200. }
  201. // Colliding and not flying with an upward force. The avatar must be trying to jump.
  202. if (!m_controllingPrim.Flying && m_controllingPrim.IsColliding && stepVelocity.Z > 0)
  203. {
  204. // We allow the upward force to happen for this many frames.
  205. m_jumpFrames = BSParam.AvatarJumpFrames;
  206. m_jumpVelocity = stepVelocity.Z;
  207. }
  208. // The case where the avatar is not colliding and is not flying is special.
  209. // The avatar is either falling or jumping and the user can be applying force to the avatar
  210. // (force in some direction or force up or down).
  211. // If the avatar has negative Z velocity and is not colliding, presume we're falling and keep the velocity.
  212. // If the user is trying to apply upward force but we're not colliding, assume the avatar
  213. // is trying to jump and don't apply the upward force if not touching the ground any more.
  214. if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
  215. {
  216. // If upward velocity is being applied, this must be a jump and only allow that to go on so long
  217. if (m_jumpFrames > 0)
  218. {
  219. // Since not touching the ground, only apply upward force for so long.
  220. m_jumpFrames--;
  221. stepVelocity.Z = m_jumpVelocity;
  222. }
  223. else
  224. {
  225. // Since we're not affected by anything, whatever vertical motion the avatar has, continue that.
  226. stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
  227. }
  228. // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
  229. }
  230. // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
  231. OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;
  232. // Add special movement force to allow avatars to walk up stepped surfaces.
  233. moveForce += WalkUpStairs();
  234. m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}",
  235. m_controllingPrim.LocalID, stepVelocity, m_controllingPrim.RawVelocity, m_controllingPrim.Mass, moveForce);
  236. m_physicsScene.PE.ApplyCentralImpulse(m_controllingPrim.PhysBody, moveForce);
  237. }
  238. }
  239. // Called just as the property update is received from the physics engine.
  240. // Do any mode necessary for avatar movement.
  241. private void Process_OnPreUpdateProperty(ref EntityProperties entprop)
  242. {
  243. // Don't change position if standing on a stationary object.
  244. if (m_controllingPrim.IsStationary)
  245. {
  246. entprop.Position = m_controllingPrim.RawPosition;
  247. m_physicsScene.PE.SetTranslation(m_controllingPrim.PhysBody, entprop.Position, entprop.Rotation);
  248. }
  249. }
  250. // Decide if the character is colliding with a low object and compute a force to pop the
  251. // avatar up so it can walk up and over the low objects.
  252. private OMV.Vector3 WalkUpStairs()
  253. {
  254. OMV.Vector3 ret = OMV.Vector3.Zero;
  255. m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4},avHeight={5}",
  256. m_controllingPrim.LocalID, m_controllingPrim.IsColliding, m_controllingPrim.Flying,
  257. m_controllingPrim.TargetVelocitySpeed, m_controllingPrim.CollisionsLastTick.Count, m_controllingPrim.Size.Z);
  258. // Check for stairs climbing if colliding, not flying and moving forward
  259. if ( m_controllingPrim.IsColliding
  260. && !m_controllingPrim.Flying
  261. && m_controllingPrim.TargetVelocitySpeed > 0.1f )
  262. {
  263. // The range near the character's feet where we will consider stairs
  264. // float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) + 0.05f;
  265. // Note: there is a problem with the computation of the capsule height. Thus RawPosition is off
  266. // from the height. Revisit size and this computation when height is scaled properly.
  267. float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) - 0.05f;
  268. float nearFeetHeightMax = nearFeetHeightMin + BSParam.AvatarStepHeight;
  269. // Look for a collision point that is near the character's feet and is oriented the same as the charactor is.
  270. // Find the highest 'good' collision.
  271. OMV.Vector3 highestTouchPosition = OMV.Vector3.Zero;
  272. foreach (KeyValuePair<uint, ContactPoint> kvp in m_controllingPrim.CollisionsLastTick.m_objCollisionList)
  273. {
  274. // Don't care about collisions with the terrain
  275. if (kvp.Key > m_physicsScene.TerrainManager.HighestTerrainID)
  276. {
  277. BSPhysObject collisionObject;
  278. if (m_physicsScene.PhysObjects.TryGetValue(kvp.Key, out collisionObject))
  279. {
  280. if (!collisionObject.IsVolumeDetect)
  281. {
  282. OMV.Vector3 touchPosition = kvp.Value.Position;
  283. m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
  284. m_controllingPrim.LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
  285. if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
  286. {
  287. // This contact is within the 'near the feet' range.
  288. // The normal should be our contact point to the object so it is pointing away
  289. // thus the difference between our facing orientation and the normal should be small.
  290. OMV.Vector3 directionFacing = OMV.Vector3.UnitX * m_controllingPrim.RawOrientation;
  291. OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
  292. float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
  293. if (diff < BSParam.AvatarStepApproachFactor)
  294. {
  295. if (highestTouchPosition.Z < touchPosition.Z)
  296. highestTouchPosition = touchPosition;
  297. }
  298. }
  299. }
  300. }
  301. }
  302. }
  303. m_walkingUpStairs = 0;
  304. // If there is a good step sensing, move the avatar over the step.
  305. if (highestTouchPosition != OMV.Vector3.Zero)
  306. {
  307. // Remember that we are going up stairs. This is needed because collisions
  308. // will stop when we move up so this smoothes out that effect.
  309. m_walkingUpStairs = BSParam.AvatarStepSmoothingSteps;
  310. m_lastStepUp = highestTouchPosition.Z - nearFeetHeightMin;
  311. ret = ComputeStairCorrection(m_lastStepUp);
  312. m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},ret={3}",
  313. m_controllingPrim.LocalID, highestTouchPosition, nearFeetHeightMin, ret);
  314. }
  315. }
  316. else
  317. {
  318. // If we used to be going up stairs but are not now, smooth the case where collision goes away while
  319. // we are bouncing up the stairs.
  320. if (m_walkingUpStairs > 0)
  321. {
  322. m_walkingUpStairs--;
  323. ret = ComputeStairCorrection(m_lastStepUp);
  324. }
  325. }
  326. return ret;
  327. }
  328. private OMV.Vector3 ComputeStairCorrection(float stepUp)
  329. {
  330. OMV.Vector3 ret = OMV.Vector3.Zero;
  331. OMV.Vector3 displacement = OMV.Vector3.Zero;
  332. if (stepUp > 0f)
  333. {
  334. // Found the stairs contact point. Push up a little to raise the character.
  335. if (BSParam.AvatarStepForceFactor > 0f)
  336. {
  337. float upForce = stepUp * m_controllingPrim.Mass * BSParam.AvatarStepForceFactor;
  338. ret = new OMV.Vector3(0f, 0f, upForce);
  339. }
  340. // Also move the avatar up for the new height
  341. if (BSParam.AvatarStepUpCorrectionFactor > 0f)
  342. {
  343. // Move the avatar up related to the height of the collision
  344. displacement = new OMV.Vector3(0f, 0f, stepUp * BSParam.AvatarStepUpCorrectionFactor);
  345. m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
  346. }
  347. else
  348. {
  349. if (BSParam.AvatarStepUpCorrectionFactor < 0f)
  350. {
  351. // Move the avatar up about the specified step height
  352. displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight);
  353. m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
  354. }
  355. }
  356. m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs.ComputeStairCorrection,disp={1},force={2}",
  357. m_controllingPrim.LocalID, displacement, ret);
  358. }
  359. return ret;
  360. }
  361. }
  362. }