/Aurora/Physics/AuroraOpenDynamicsEngine/AODEPrim.cs
C# | 2988 lines | 2302 code | 400 blank | 286 comment | 430 complexity | 0bfd8d4544f79359a43b71071cd95f83 MD5 | raw file
Large files files are truncated, but you can click here to view the full file
1/* 2 * Copyright (c) Contributors, http://aurora-sim.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 Aurora-Sim 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 28/* 29 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 31 * ODEPrim.cs contains methods dealing with Prim editing, Prim 32 * characteristics and Kinetic motion. 33 * ODEDynamics.cs contains methods dealing with Prim Physical motion 34 * (dynamics) and the associated settings. Old Linear and angular 35 * motors for dynamic motion have been replace with MoveLinear() 36 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 37 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to 38 * switch between 'VEHICLE' parameter use and general dynamics 39 * settings use. 40 */ 41 42using Aurora.Framework; 43using Aurora.Framework.ConsoleFramework; 44using Aurora.Framework.Physics; 45using Aurora.Framework.SceneInfo; 46using Aurora.Framework.Utilities; 47using OdeAPI; 48using OpenMetaverse; 49using System; 50using System.Collections.Generic; 51using System.Linq; 52 53//using Ode.NET; 54 55namespace Aurora.Physics.AuroraOpenDynamicsEngine 56{ 57 /// <summary> 58 /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. 59 /// </summary> 60 public class AuroraODEPrim : PhysicsObject 61 { 62 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom 63 | CollisionCategories.Space 64 | CollisionCategories.Body 65 | CollisionCategories.Character 66 ); 67 68 private static readonly Dictionary<ulong, IntPtr> m_MeshToTriMeshMap = new Dictionary<ulong, IntPtr>(); 69 private readonly AuroraODEPhysicsScene _parent_scene; 70 71 private readonly Vector3 _torque = Vector3.Zero; 72 private readonly int body_autodisable_frames = 20; 73 private readonly AuroraODEDynamics m_vehicle; 74 private IntPtr Amotor = IntPtr.Zero; 75 public IntPtr Body { get; private set; } 76 private CollisionEventUpdate CollisionEventsThisFrame; 77 78 private float PID_D = 35f; 79 private float PID_G = 25f; 80 private Vector3 _acceleration; 81 private float _mass; // prim or object mass 82 private IMesh _mesh; 83 private Quaternion _orientation; 84 private PhysicsObject _parent; 85 private ISceneChildEntity _parent_entity; 86 private PrimitiveBaseShape _pbs; 87 private Vector3 _position; 88 private Vector3 _size; 89 private IntPtr _triMeshData; 90 private Vector3 _velocity; 91 private bool _zeroFlag; 92 private bool _zeroFlagForceSet; 93 internal volatile bool childPrim; 94 internal List<AuroraODEPrim> childrenPrim = new List<AuroraODEPrim>(); 95 private int fakeori; // control the use of above 96 private int fakepos; // control the use of above 97 private bool hasOOBoffsetFromMesh; // if true we did compute it form mesh centroid, else from aabb 98 private bool iscolliding; 99 private Vector3 m_angularforceacc; 100 private Vector3 m_angularlock = Vector3.One; 101 private bool m_blockPhysicalReconstruction; 102 private bool m_buildingRepresentation; 103 104 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 105 // and are for non-VEHICLES only. 106 107 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 108 109 // private float m_tensor = 5f; 110 111 private bool m_collidesLand = true; 112 private bool m_collidesWater; 113 114 // Default we're a Geometry 115 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 116 117 // Default, Collide with Other Geometries, spaces and Bodies 118 private CollisionCategories m_collisionFlags = m_default_collisionFlags; 119 internal float m_collisionscore; 120 private int m_crossingfailures; 121 122 internal bool m_disabled; 123 private bool m_eventsubscription; 124 //This disables the prim so that it cannot do much anything at all 125 126 private Vector3 m_force; 127 private Vector3 m_forceacc; 128 internal bool m_frozen; 129 private bool m_isSelected; 130 131 private bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively 132 private bool m_isphysical; 133 private int m_lastUpdateSent; 134 private Vector3 m_lastVelocity; 135 private Quaternion m_lastorientation; 136 private Vector3 m_lastposition; 137 private uint m_localID; 138 private int m_material = (int) Material.Wood; 139 private bool m_primIsRemoved; 140 private Vector3 m_pushForce; 141 private Vector3 m_rotationalVelocity; 142 internal IntPtr m_targetSpace = IntPtr.Zero; 143 144 private bool m_throttleUpdates; 145 146 private float primMass; // prim own mass 147 148 public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb 149 public Vector3 primOOBsize; // prim real dimensions from mesh 150 public IntPtr prim_geom; 151 private d.Mass primdMass; // prim inertia information on it's own referencial 152 private Quaternion showorientation; // tmp hack see showposition 153 private Vector3 showposition; // a temp hack for now rest of code expects position to be changed immediately 154 155 public AuroraODEPrim(ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical) 156 { 157 m_vehicle = new AuroraODEDynamics(); 158 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); 159 160 // correct for changed timestep 161 PID_D /= (parent_scene.ODE_STEPSIZE*50f); // original ode fps of 50 162 PID_G /= (parent_scene.ODE_STEPSIZE*50f); 163 164 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 165 166 prim_geom = IntPtr.Zero; 167 168 _size = entity.Scale; 169 _position = entity.AbsolutePosition; 170 fakepos = 0; 171 _orientation = entity.GetWorldRotation(); 172 fakeori = 0; 173 _pbs = entity.Shape; 174 _parent_entity = entity; 175 176 _parent_scene = parent_scene; 177 m_targetSpace = IntPtr.Zero; 178 179 /* 180 m_isphysical = pisPhysical; 181 if (m_isphysical) 182 m_targetSpace = _parent_scene.space; 183 */ 184 m_isphysical = false; 185 186 m_forceacc = Vector3.Zero; 187 m_angularforceacc = Vector3.Zero; 188 189 hasOOBoffsetFromMesh = false; 190 _triMeshData = IntPtr.Zero; 191 192 CalcPrimBodyData(); 193 194 _parent_scene.AddSimulationChange(() => changeadd()); 195 } 196 197 public ISceneChildEntity ParentEntity 198 { 199 get { return _parent_entity; } 200 } 201 202 public override bool BuildingRepresentation 203 { 204 get { return m_buildingRepresentation; } 205 set 206 { 207 if (value) 208 m_buildingRepresentation = value; 209 //else 210 // _parent_scene.AddSimulationChange(() => m_buildingRepresentation = false); 211 } 212 } 213 214 public override bool BlockPhysicalReconstruction 215 { 216 get { return m_blockPhysicalReconstruction; } 217 set 218 { 219 if (value) 220 m_blockPhysicalReconstruction = value; 221 else 222 _parent_scene.AddSimulationChange(() => 223 { 224 if (value) 225 DestroyBody(); 226 else 227 { 228 m_blockPhysicalReconstruction = false; 229 if (!childPrim) 230 MakeBody(); 231 } 232 if (!childPrim && childrenPrim.Count > 0) 233 { 234 foreach (AuroraODEPrim prm in childrenPrim) 235 prm.BlockPhysicalReconstruction = value; 236 } 237 }); 238 } 239 } 240 241 public PhysicsObject Parent 242 { 243 get { return _parent; } 244 } 245 246 public override int PhysicsActorType 247 { 248 get { return (int) ActorTypes.Prim; } 249 } 250 251 public override uint LocalID 252 { 253 get { return m_localID; } 254 set 255 { 256 //MainConsole.Instance.Info("[PHYSICS]: Setting TrackerID: " + value); 257 m_localID = value; 258 } 259 } 260 261 public override bool VolumeDetect 262 { 263 get { return m_isVolumeDetect; } 264 set { _parent_scene.AddSimulationChange(() => changevoldtc(value)); } 265 } 266 267 public override bool Selected 268 { 269 set 270 { 271 // This only makes the object not collidable if the object 272 // is physical or the object is modified somehow *IN THE FUTURE* 273 // without this, if an avatar selects prim, they can walk right 274 // through it while it's selected 275 if ((IsPhysical && !_zeroFlag) || !value) 276 _parent_scene.AddSimulationChange(() => changeSelectedStatus(value)); 277 else 278 m_isSelected = value; 279 if (m_isSelected) 280 disableBodySoft(); 281 } 282 } 283 284 public override bool IsPhysical 285 { 286 get 287 { 288 if (childPrim && _parent != null) // root prim defines if is physical or not 289 return ((AuroraODEPrim) _parent).m_isphysical; 290 else 291 return m_isphysical; 292 } 293 set 294 { 295 _parent_scene.AddSimulationChange(() => changePhysicsStatus(value)); 296 if (!value) // Zero the remembered last velocity 297 m_lastVelocity = Vector3.Zero; 298 } 299 } 300 301 public override bool IsTruelyColliding { get; set; } 302 303 public override bool IsColliding 304 { 305 get { return iscolliding; } 306 set 307 { 308 if (value && _parent != null) 309 _parent.LinkSetIsColliding = value; 310 LinkSetIsColliding = value; 311 iscolliding = value; 312 } 313 } 314 315 public override bool LinkSetIsColliding { get; set; } 316 317 public override bool ThrottleUpdates 318 { 319 get { return m_throttleUpdates; } 320 set { m_throttleUpdates = value; } 321 } 322 323 public override Vector3 Position 324 { 325 get 326 { 327 if (fakepos > 0) 328 return showposition; 329 else 330 return _position; 331 } 332 set 333 { 334 showposition = value; 335 fakepos++; 336 _parent_scene.AddSimulationChange(() => changePosition(value)); 337 } 338 } 339 340 public override Vector3 Size 341 { 342 get { return _size; } 343 set 344 { 345 if (value.IsFinite()) 346 _parent_scene.AddSimulationChange(() => changesize(value)); 347 else 348 MainConsole.Instance.Warn("[PHYSICS]: Got NaN Size on object"); 349 } 350 } 351 352 353 public override float Mass 354 { 355 get { return _mass; } 356 } 357 358 public override Vector3 Force 359 { 360 //get { return Vector3.Zero; } 361 get { return m_force; } 362 set 363 { 364 if (value.IsFinite()) 365 { 366 _parent_scene.AddSimulationChange(() => changeforce(value, false)); 367 } 368 else 369 { 370 MainConsole.Instance.Warn("[PHYSICS]: NaN in Force Applied to an Object"); 371 } 372 } 373 } 374 375 public override int VehicleType 376 { 377 get { return (int) m_vehicle.Type; } 378 set { _parent_scene.AddSimulationChange(() => changeVehicleType(value)); } 379 } 380 381 public override Vector3 CenterOfMass 382 { 383 get 384 { 385 d.Vector3 dtmp; 386 if (IsPhysical && !childPrim && Body != IntPtr.Zero) 387 { 388 dtmp = d.BodyGetPosition(Body); 389 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); 390 } 391 else if (prim_geom != IntPtr.Zero) 392 { 393 d.Quaternion dq; 394 d.GeomCopyQuaternion(prim_geom, out dq); 395 Quaternion q; 396 q.X = dq.X; 397 q.Y = dq.Y; 398 q.Z = dq.Z; 399 q.W = dq.W; 400 401 Vector3 vtmp = primOOBoffset*q; 402 dtmp = d.GeomGetPosition(prim_geom); 403 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); 404 } 405 else 406 return Vector3.Zero; 407 } 408 } 409 410 public override PrimitiveBaseShape Shape 411 { 412 set { _parent_scene.AddSimulationChange(() => changeshape(value)); } 413 } 414 415 public override Vector3 Velocity 416 { 417 get 418 { 419 // Averate previous velocity with the new one so 420 // client object interpolation works a 'little' better 421 if (_zeroFlag) 422 return Vector3.Zero; 423 424 Vector3 returnVelocity = Vector3.Zero; 425 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; 426 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; 427 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; 428 return returnVelocity; 429 } 430 set 431 { 432 if (value.IsFinite()) 433 _parent_scene.AddSimulationChange(() => changevelocity(value)); 434 else 435 { 436 MainConsole.Instance.Warn("[PHYSICS]: Got NaN Velocity in Object"); 437 } 438 } 439 } 440 441 public override Vector3 Torque 442 { 443 get 444 { 445 if (childPrim || !m_isphysical || Body == IntPtr.Zero) 446 return Vector3.Zero; 447 448 return _torque; 449 } 450 451 set 452 { 453 if (value.IsFinite()) 454 _parent_scene.AddSimulationChange(() => changeSetTorque(value)); 455 else 456 MainConsole.Instance.Warn("[PHYSICS]: Got NaN Torque in Object"); 457 } 458 } 459 460 public override float CollisionScore 461 { 462 get { return m_collisionscore; } 463 set { m_collisionscore = value; } 464 } 465 466 public override Quaternion Orientation 467 { 468 get 469 { 470 if (fakeori > 0) 471 return showorientation; 472 else 473 return _orientation; 474 } 475 set 476 { 477 if (QuaternionIsFinite(value)) 478 { 479 showorientation = value; 480 fakeori++; 481 _parent_scene.AddSimulationChange(() => changeOrientation(value)); 482 } 483 else 484 MainConsole.Instance.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 485 } 486 } 487 488 public override Vector3 Acceleration 489 { 490 get { return _acceleration; } 491 } 492 493 public override Vector3 RotationalVelocity 494 { 495 get 496 { 497 if (_zeroFlag) 498 return Vector3.Zero; 499 500 if (m_rotationalVelocity.ApproxEquals(Vector3.Zero, 0.00001f)) 501 return Vector3.Zero; 502 503 return m_rotationalVelocity; 504 } 505 set 506 { 507 if (value.IsFinite()) 508 _parent_scene.AddSimulationChange(() => changeangvelocity(value)); 509 else 510 MainConsole.Instance.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); 511 } 512 } 513 514 public override float Buoyancy 515 { 516 get { return m_buoyancy; } 517 set { m_buoyancy = value; } 518 } 519 520 public override bool FloatOnWater 521 { 522 set { _parent_scene.AddSimulationChange(() => changefloatonwater(value)); } 523 } 524 525 public override void ClearVelocity() 526 { 527 _velocity = Vector3.Zero; 528 _acceleration = Vector3.Zero; 529 m_rotationalVelocity = Vector3.Zero; 530 m_lastorientation = Orientation; 531 m_lastposition = Position; 532 m_lastVelocity = _velocity; 533 if (Body != null && Body != IntPtr.Zero) 534 d.BodySetLinearVel(Body, 0, 0, 0); 535 } 536 537 public override void ForceSetVelocity(Vector3 velocity) 538 { 539 _velocity = velocity; 540 m_lastVelocity = velocity; 541 if (Body != IntPtr.Zero) 542 d.BodySetLinearVel(Body, velocity.X, velocity.Y, velocity.Z); 543 } 544 545 public void ForceSetRotVelocity(Vector3 velocity) 546 { 547 m_rotationalVelocity = velocity; 548 if (Body != IntPtr.Zero) 549 d.BodySetAngularVel(Body, velocity.X, velocity.Y, velocity.Z); 550 } 551 552 public void SetGeom(IntPtr geom) 553 { 554 prim_geom = geom; 555 //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 556 if (prim_geom != IntPtr.Zero) 557 { 558 d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories); 559 d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags); 560 561 CalcPrimBodyData(); 562 563 _parent_scene.actor_name_map[prim_geom] = this; 564 } 565 566 if (childPrim) 567 { 568 if (_parent != null && _parent is AuroraODEPrim) 569 { 570 AuroraODEPrim parent = (AuroraODEPrim) _parent; 571 //Console.WriteLine("SetGeom calls ChildSetGeom"); 572 parent.ChildSetGeom(this); 573 } 574 } 575 //MainConsole.Instance.Warn("Setting Geom to: " + prim_geom); 576 } 577 578 public void enableBodySoft() 579 { 580 if (!childPrim) 581 { 582 if (m_isphysical && Body != IntPtr.Zero) 583 { 584 d.BodyEnable(Body); 585 if (m_vehicle.Type != Vehicle.TYPE_NONE) 586 m_vehicle.Enable(Body, this, _parent_scene); 587 } 588 589 m_disabled = false; 590 } 591 } 592 593 public void disableBodySoft() 594 { 595 if (!childPrim) 596 { 597 m_disabled = true; 598 m_vehicle.Disable(this); 599 if (IsPhysical && Body != IntPtr.Zero) 600 d.BodyDisable(Body); 601 } 602 } 603 604 private void MakeBody() 605 { 606 // d.Vector3 dvtmp; 607 // d.Vector3 dbtmp; 608 609 610 if (m_blockPhysicalReconstruction) // building is blocked 611 return; 612 613 if (childPrim) // child prims don't get own bodies; 614 return; 615 616 if (prim_geom == IntPtr.Zero) 617 { 618 MainConsole.Instance.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); 619 return; 620 } 621 622 if (!m_isphysical) // only physical things get a body 623 return; 624 625 if (Body != IntPtr.Zero) // who shouldn't have one already ? 626 { 627 d.BodyDestroy(Body); 628 Body = IntPtr.Zero; 629 MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body"); 630 } 631 632 633 d.Mass objdmass = new d.Mass {}; 634 d.Matrix3 mymat = new d.Matrix3(); 635 d.Quaternion myrot = new d.Quaternion(); 636 637 Body = d.BodyCreate(_parent_scene.world); 638 639 DMassDup(ref primdMass, out objdmass); 640 641 // rotate inertia 642 myrot.X = _orientation.X; 643 myrot.Y = _orientation.Y; 644 myrot.Z = _orientation.Z; 645 myrot.W = _orientation.W; 646 647 d.RfromQ(out mymat, ref myrot); 648 d.MassRotate(ref objdmass, ref mymat); 649 650 // set the body rotation and position 651 d.BodySetRotation(Body, ref mymat); 652 653 // recompute full object inertia if needed 654 if (childrenPrim.Count > 0) 655 { 656 d.Matrix3 mat = new d.Matrix3(); 657 d.Quaternion quat = new d.Quaternion(); 658 d.Mass tmpdmass = new d.Mass {}; 659 Vector3 rcm; 660 661 rcm.X = _position.X + objdmass.c.X; 662 rcm.Y = _position.Y + objdmass.c.Y; 663 rcm.Z = _position.Z + objdmass.c.Z; 664 665 lock (childrenPrim) 666 { 667 foreach (AuroraODEPrim prm in childrenPrim) 668 { 669 if (prm.prim_geom == IntPtr.Zero) 670 { 671 MainConsole.Instance.Warn( 672 "[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); 673 continue; 674 } 675 676 DMassCopy(ref prm.primdMass, ref tmpdmass); 677 678 // apply prim current rotation to inertia 679 quat.W = prm._orientation.W; 680 quat.X = prm._orientation.X; 681 quat.Y = prm._orientation.Y; 682 quat.Z = prm._orientation.Z; 683 d.RfromQ(out mat, ref quat); 684 d.MassRotate(ref tmpdmass, ref mat); 685 686 Vector3 ppos = prm._position; 687 ppos.X += tmpdmass.c.X - rcm.X; 688 ppos.Y += tmpdmass.c.Y - rcm.Y; 689 ppos.Z += tmpdmass.c.Z - rcm.Z; 690 691 // refer inertia to root prim center of mass position 692 d.MassTranslate(ref tmpdmass, 693 ppos.X, 694 ppos.Y, 695 ppos.Z); 696 697 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia 698 699 // fix prim colision cats 700 701 d.GeomClearOffset(prm.prim_geom); 702 d.GeomSetBody(prm.prim_geom, Body); 703 prm.Body = Body; 704 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation 705 } 706 } 707 } 708 709 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset 710 // associate root geom with body 711 d.GeomSetBody(prim_geom, Body); 712 713 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); 714 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); 715 716 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); 717 // ode wants inertia at center of body 718 myrot.W = -myrot.W; 719 d.RfromQ(out mymat, ref myrot); 720 d.MassRotate(ref objdmass, ref mymat); 721 d.BodySetMass(Body, ref objdmass); 722 _mass = objdmass.mass; 723 724 m_collisionCategories |= CollisionCategories.Body; 725 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 726 727 // disconnect from world gravity so we can apply buoyancy 728 // if (!testRealGravity) 729 d.BodySetGravityMode(Body, false); 730 731 d.BodySetAutoDisableFlag(Body, true); 732 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 733 d.BodySetDamping(Body, .001f, .0002f); 734 m_disabled = false; 735 736 d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories); 737 d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags); 738 739 if (m_targetSpace != _parent_scene.space) 740 { 741 if (d.SpaceQuery(m_targetSpace, prim_geom)) 742 d.SpaceRemove(m_targetSpace, prim_geom); 743 744 m_targetSpace = _parent_scene.space; 745 d.SpaceAdd(m_targetSpace, prim_geom); 746 } 747 748 lock (childrenPrim) 749 { 750 foreach (AuroraODEPrim prm in childrenPrim) 751 { 752 if (prm.prim_geom == IntPtr.Zero) 753 continue; 754 755 Vector3 ppos = prm._position; 756 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position 757 758 prm.m_collisionCategories |= CollisionCategories.Body; 759 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 760 d.GeomSetCategoryBits(prm.prim_geom, (int) prm.m_collisionCategories); 761 d.GeomSetCollideBits(prm.prim_geom, (int) prm.m_collisionFlags); 762 763 764 if (prm.m_targetSpace != _parent_scene.space) 765 { 766 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 767 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); 768 769 prm.m_targetSpace = _parent_scene.space; 770 d.SpaceAdd(m_targetSpace, prm.prim_geom); 771 } 772 773 prm.m_disabled = false; 774 _parent_scene.addActivePrim(prm); 775 } 776 } 777 // The body doesn't already have a finite rotation mode set here 778 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) 779 { 780 createAMotor(m_angularlock); 781 } 782 if (m_vehicle.Type != Vehicle.TYPE_NONE) 783 m_vehicle.Enable(Body, this, _parent_scene); 784 785 _parent_scene.addActivePrim(this); 786 } 787 788 private void SetInStaticSpace(AuroraODEPrim prm) 789 { 790 if (prm.m_targetSpace != null && prm.m_targetSpace == _parent_scene.space) 791 { 792 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 793 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); 794 } 795 prm.m_targetSpace = _parent_scene.calculateSpaceForGeom(prm._position); 796 d.SpaceAdd(prm.m_targetSpace, prm.prim_geom); 797 } 798 799 800 public void DestroyBody() 801 // for now removes all colisions etc from childs, full body reconstruction is needed after this 802 { 803 //this kills the body so things like 'mesh' can re-create it. 804 lock (this) 805 { 806 if (Body != IntPtr.Zero) 807 { 808 _parent_scene.remActivePrim(this); 809 m_collisionCategories &= ~CollisionCategories.Body; 810 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 811 if (prim_geom != IntPtr.Zero) 812 { 813 d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories); 814 d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags); 815 UpdateDataFromGeom(); 816 SetInStaticSpace(this); 817 } 818 if (!childPrim) 819 { 820 lock (childrenPrim) 821 { 822 foreach (AuroraODEPrim prm in childrenPrim) 823 { 824 _parent_scene.remActivePrim(prm); 825 prm.m_collisionCategories &= ~CollisionCategories.Body; 826 prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 827 if (prm.prim_geom != IntPtr.Zero) 828 { 829 d.GeomSetCategoryBits(prm.prim_geom, (int) m_collisionCategories); 830 d.GeomSetCollideBits(prm.prim_geom, (int) m_collisionFlags); 831 prm.UpdateDataFromGeom(); 832 prm.Body = IntPtr.Zero; 833 SetInStaticSpace(prm); 834 } 835 prm._mass = prm.primMass; 836 } 837 } 838 m_vehicle.Disable(this); 839 d.BodyDestroy(Body); 840 } 841 } 842 Body = IntPtr.Zero; 843 } 844 _mass = primMass; 845 m_disabled = true; 846 } 847 848 public bool setMesh(AuroraODEPhysicsScene parent_scene, IMesh mesh) 849 { 850 // This sleeper is there to moderate how long it takes between 851 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object 852 853 //Thread.Sleep(10); 854 855 //Kill Body so that mesh can re-make the geom 856 if (IsPhysical && Body != IntPtr.Zero) 857 { 858 if (childPrim) 859 { 860 if (_parent != null) 861 { 862 AuroraODEPrim parent = (AuroraODEPrim) _parent; 863 parent.ChildDelink(this); 864 } 865 } 866 else 867 { 868 DestroyBody(); 869 } 870 } 871 872 IntPtr vertices, indices; 873 int vertexCount, indexCount; 874 int vertexStride, triStride; 875 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); 876 // Note, that vertices are fixed in unmanaged heap 877 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); 878 // Also fixed, needs release after usage 879 880 if (vertexCount == 0 || indexCount == 0) 881 { 882 MainConsole.Instance.WarnFormat( 883 "[PHYSICS]: Got invalid mesh on prim at <{0},{1},{2}>. It can be a sculpt with alpha channel in map. Replacing it by a small box.", 884 _position.X, _position.Y, _position.Z); 885 _size.X = 0.01f; 886 _size.Y = 0.01f; 887 _size.Z = 0.01f; 888 return false; 889 } 890 891 primOOBoffset = mesh.GetCentroid(); 892 hasOOBoffsetFromMesh = true; 893 894 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory 895 if (m_MeshToTriMeshMap.ContainsKey(mesh.Key)) 896 { 897 _triMeshData = m_MeshToTriMeshMap[mesh.Key]; 898 } 899 else 900 { 901 _triMeshData = d.GeomTriMeshDataCreate(); 902 903 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, 904 triStride); 905 d.GeomTriMeshDataPreprocess(_triMeshData); 906 m_MeshToTriMeshMap[mesh.Key] = _triMeshData; 907 } 908 909 try 910 { 911 if (prim_geom == IntPtr.Zero) 912 { 913 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); 914 } 915 } 916 catch (AccessViolationException) 917 { 918 MainConsole.Instance.Error("[PHYSICS]: MESH LOCKED"); 919 return false; 920 } 921 922 return true; 923 } 924 925 private void changeAngularLock(Vector3 newlock) 926 { 927 // do we have a Physical object? 928 if (Body != IntPtr.Zero) 929 { 930 //Check that we have a Parent 931 //If we have a parent then we're not authorative here 932 if (_parent == null) 933 { 934 if (!newlock.ApproxEquals(Vector3.One, 0f)) 935 { 936 createAMotor(newlock); 937 } 938 else 939 { 940 if (Amotor != IntPtr.Zero) 941 { 942 d.JointDestroy(Amotor); 943 Amotor = IntPtr.Zero; 944 } 945 } 946 } 947 } 948 // Store this for later in case we get turned into a separate body 949 m_angularlock = newlock; 950 } 951 952 private void changelink(AuroraODEPrim newparent) 953 { 954 // If the newly set parent is not null 955 // create link 956 if (_parent == null && newparent != null) 957 { 958 newparent.ParentPrim(this); 959 } 960 // If the newly set parent is null 961 // destroy link 962 else if (_parent != null) 963 { 964 if (_parent is AuroraODEPrim) 965 { 966 if (newparent != _parent) 967 { 968 AuroraODEPrim obj = (AuroraODEPrim) _parent; 969 obj.ChildDelink(this); 970 childPrim = false; 971 972 if (newparent != null) 973 { 974 newparent.ParentPrim(this); 975 } 976 } 977 } 978 } 979 980 _parent = newparent; 981 } 982 983 // I'm the parent 984 // prim is the child 985 public void ParentPrim(AuroraODEPrim prim) 986 { 987 //Console.WriteLine("ParentPrim " + m_primName); 988 if (this.m_localID != prim.m_localID) 989 { 990 DestroyBody(); 991 992 lock (childrenPrim) 993 { 994#if (!ISWIN) 995 foreach (AuroraODEPrim prm in prim.childrenPrim) 996 { 997 if (!childrenPrim.Contains(prm)) 998 { 999 childrenPrim.Add(prm); 1000 } 1001 } 1002#else 1003 foreach (AuroraODEPrim prm in prim.childrenPrim.Where(prm => !childrenPrim.Contains(prm))) 1004 { 1005 childrenPrim.Add(prm); 1006 } 1007#endif 1008 if (!childrenPrim.Contains(prim)) // must allow full reconstruction 1009 childrenPrim.Add(prim); 1010 } 1011 //Remove old children 1012 prim.childrenPrim.Clear(); 1013 prim.childPrim = true; 1014 prim._parent = this; 1015 1016 if (prim.Body != IntPtr.Zero) 1017 { 1018 prim.DestroyBody(); // don't loose bodies around 1019 prim.Body = IntPtr.Zero; 1020 } 1021 MakeBody(); // full nasty reconstruction 1022 } 1023 } 1024 1025 private void ChildSetGeom(AuroraODEPrim odePrim) 1026 { 1027 DestroyBody(); 1028 MakeBody(); 1029 } 1030 1031 private void UpdateChildsfromgeom() 1032 { 1033 if (childrenPrim.Count > 0) 1034 { 1035 foreach (AuroraODEPrim prm in childrenPrim) 1036 prm.UpdateDataFromGeom(); 1037 } 1038 } 1039 1040 private void UpdateDataFromGeom() 1041 { 1042 if (prim_geom != IntPtr.Zero) 1043 { 1044 d.Vector3 lpos = d.GeomGetPosition(prim_geom); 1045 _position.X = lpos.X; 1046 _position.Y = lpos.Y; 1047 _position.Z = lpos.Z; 1048 d.Quaternion qtmp = new d.Quaternion 1049 { 1050 }; 1051 d.GeomCopyQuaternion(prim_geom, out qtmp); 1052 _orientation.W = qtmp.W; 1053 _orientation.X = qtmp.X; 1054 _orientation.Y = qtmp.Y; 1055 _orientation.Z = qtmp.Z; 1056 } 1057 } 1058 1059 private void ChildDelink(AuroraODEPrim odePrim) 1060 { 1061 // Okay, we have a delinked child.. destroy all body and remake 1062 if (odePrim != this && !childrenPrim.Contains(odePrim)) 1063 return; 1064 1065 DestroyBody(); 1066 1067 if (odePrim == this) 1068 { 1069 AuroraODEPrim newroot = null; 1070 lock (childrenPrim) 1071 { 1072 if (childrenPrim.Count > 0) 1073 { 1074 newroot = childrenPrim[0]; 1075 childrenPrim.RemoveAt(0); 1076 foreach (AuroraODEPrim prm in childrenPrim) 1077 { 1078 newroot.childrenPrim.Add(prm); 1079 } 1080 childrenPrim.Clear(); 1081 } 1082 if (newroot != null) 1083 { 1084 newroot.childPrim = false; 1085 newroot._parent = null; 1086 newroot.MakeBody(); 1087 } 1088 } 1089 } 1090 1091 else 1092 { 1093 lock (childrenPrim) 1094 { 1095 childrenPrim.Remove(odePrim); 1096 odePrim.childPrim = false; 1097 odePrim._parent = null; 1098 //odePrim.UpdateDataFromGeom (); 1099 odePrim.MakeBody(); 1100 } 1101 } 1102 1103 MakeBody(); 1104 } 1105 1106 private void ChildRemove(AuroraODEPrim odePrim) 1107 { 1108 // Okay, we have a delinked child.. destroy all body and remake 1109 if (odePrim != this && !childrenPrim.Contains(odePrim)) 1110 return; 1111 1112 DestroyBody(); 1113 1114 if (odePrim == this) 1115 { 1116 AuroraODEPrim newroot = null; 1117 lock (childrenPrim) 1118 { 1119 if (childrenPrim.Count > 0) 1120 { 1121 newroot = childrenPrim[0]; 1122 childrenPrim.RemoveAt(0); 1123 foreach (AuroraODEPrim prm in childrenPrim) 1124 { 1125 newroot.childrenPrim.Add(prm); 1126 } 1127 childrenPrim.Clear(); 1128 } 1129 if (newroot != null) 1130 { 1131 newroot.childPrim = false; 1132 newroot._parent = null; 1133 newroot.MakeBody(); 1134 } 1135 } 1136 return; 1137 } 1138 else 1139 { 1140 lock (childrenPrim) 1141 { 1142 childrenPrim.Remove(odePrim); 1143 odePrim.childPrim = false; 1144 odePrim._parent = null; 1145 } 1146 } 1147 1148 MakeBody(); 1149 } 1150 1151 private void changeSelectedStatus(bool newsel) 1152 { 1153 bool isphys = IsPhysical; 1154 1155 if (newsel) 1156 { 1157 m_collisionCategories = CollisionCategories.Selected; 1158 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); 1159 1160 // We do the body disable soft twice because 'in theory' a collision could have happened 1161 // in between the disabling and the collision properties setting 1162 // which would wake the physical body up from a soft disabling and potentially cause it to fall 1163 // through the ground. 1164 1165 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select 1166 // just one part of the assembly, the rest of the assembly is non-selected and still simulating, 1167 // so that causes the selected part to wake up and continue moving. 1168 1169 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire 1170 // assembly will stop simulating during the selection, because of the lack of atomicity 1171 // of select operations (their processing could be interrupted by a thread switch, causing 1172 // simulation to continue before all of the selected object notifications trickle down to 1173 // the physics engine). 1174 1175 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are 1176 // selected and disabled. then, due to a thread switch, the selection processing is 1177 // interrupted and the physics engine continues to simulate, so the last 50 items, whose 1178 // selection was not yet processed, continues to simulate. this wakes up ALL of the 1179 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken 1180 // up, start simulating again, which in turn wakes up the last 50. 1181 1182 if (isphys) 1183 { 1184 disableBodySoft(); 1185 } 1186 1187 if (prim_geom != IntPtr.Zero) 1188 { 1189 d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories); 1190 d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags); 1191 } 1192 1193 if (isphys) 1194 { 1195 disableBodySoft(); 1196 } 1197 } 1198 else 1199 { 1200 m_collisionCategories = CollisionCategories.Geom; 1201 1202 if (isphys) 1203 m_collisionCategories |= CollisionCategories.Body; 1204 1205 m_collisionFlags = m_default_collisionFlags; 1206 1207 if (m_collidesLand) 1208 m_collisionFlags |= CollisionCategories.Land; 1209 if (m_collidesWater) 1210 m_collisionFlags |= CollisionCategories.Water; 1211 1212 if (prim_geom != IntPtr.Zero) 1213 { 1214 d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories); 1215 d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags); 1216 } 1217 if (isphys) 1218 { 1219 if (Body != IntPtr.Zero) 1220 { 1221 d.BodySetLinearVel(Body, 0f, 0f, 0f); 1222 d.BodySetAngularVel(Body, 0f, 0f, 0f); 1223 d.BodySetForce(Body, 0, 0, 0); 1224 d.BodySetTorque(Body, 0, 0, 0); 1225 enableBodySoft(); 1226 } 1227 } 1228 } 1229 1230 resetCollisionAccounting(); 1231 m_isSelected = newsel; 1232 if (!m_isSelected) 1233 { 1234 _zeroFlag = false; 1235 } 1236 } 1237 1238 //end changeSelectedStatus 1239 1240 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1241 { 1242 hasOOBoffsetFromMesh = false; 1243 1244 bool havemesh = false; 1245 //Console.WriteLine("CreateGeom:"); 1246 if (_mesh != null) 1247 { 1248 havemesh = setMesh(_parent_scene, _mesh); // this will give a mesh to non trivial known prims 1249 } 1250 1251 if (!havemesh) 1252 { 1253 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte) Extrusion.Curve1 1254 && _size.X == _size.Y && _size.Y == _size.Z) 1255 { 1256 // it's a sphere 1257 try 1258 { 1259 SetGeom(d.CreateSphere(m_targetSpace, _size.X*0.5f)); 1260 } 1261 catch (Exception e) 1262 { 1263 MainConsole.Instance.Warn("[PHYSICS]: Create sphere failed: {0}", e); 1264 return; 1265 } 1266 } 1267 else 1268 { 1269 try 1270 { 1271 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1272 } 1273 catch (Exception e) 1274 { 1275 MainConsole.Instance.Warn("[PHYSICS]: Create box failed: {0}", e); 1276 return; 1277 } 1278 } 1279 } 1280 } 1281 1282 private void RemoveGeom() 1283 { 1284 if (prim_geom != IntPtr.Zero) 1285 { 1286 _parent_scene.actor_name_map.Remove(prim_geom); 1287 try 1288 { 1289 d.GeomDestroy(prim_geom); 1290 /* 1291 if (_triMeshData != IntPtr.Zero) 1292 { 1293 d.GeomTriMeshDataDestroy(_triMeshData); 1294 _triMeshData = IntPtr.Zero; 1295 } 1296 */ 1297 } 1298 1299 catch (Exception e) 1300 { 1301 MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction failed {1}", e); 1302 } 1303 1304 prim_geom = IntPtr.Zero; 1305 } 1306 else 1307 { 1308 MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD"); 1309 } 1310 Body = IntPtr.Zero; 1311 hasOOBoffsetFromMesh = false; 1312 CalcPrimBodyData(); 1313 } 1314 1315 public void changeadd() 1316 { 1317 // all prims are now created non physical 1318 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); 1319 m_targetSpace = targetspace; 1320 1321 if (_mesh == null) 1322 { 1323 if (_parent_scene.needsMeshing(_parent_entity)) 1324 { 1325 // Don't need to re-enable body.. it's done in SetMesh 1326 _mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size, 1327 _parent_scene.meshSculptLOD, true); 1328 1329 //…
Large files files are truncated, but you can click here to view the full file