PageRenderTime 68ms CodeModel.GetById 32ms app.highlight 29ms RepoModel.GetById 1ms app.codeStats 0ms

/Aurora/Physics/AuroraOpenDynamicsEngine/AODEPrim.cs

https://bitbucket.org/VirtualReality/software-testing
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