public void SetRotationLocked(Quaternion taintRot)
 {
     d.Quaternion q = new d.Quaternion
     {
         W = taintRot.W,
         X = taintRot.X,
         Y = taintRot.Y,
         Z = taintRot.Z
     };
     d.BodySetQuaternion(Body, ref q); // just keep in sync with rest of simutator
 }
Exemple #2
0
        internal void LimitRotation(float timestep)
        {
            d.Quaternion rot  = d.BodyGetQuaternion(Body);
            Quaternion   rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);  // rotq = rotation of object

            d.Quaternion m_rot   = new d.Quaternion();
            bool         changed = false;

            m_rot.X = rotq.X;
            m_rot.Y = rotq.Y;
            m_rot.Z = rotq.Z;
            m_rot.W = rotq.W;
            if (m_RollreferenceFrame != Quaternion.Identity)
            {
                if (rotq.X >= m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
                }
                if (rotq.Y >= m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
                }
                if (rotq.X <= -m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
                }
                if (rotq.Y <= -m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
                }
                changed = true;
            }
            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
            {
                m_rot.X = 0;
                m_rot.Y = 0;
                changed = true;
            }
            if (changed)
            {
                d.BodySetQuaternion(Body, ref m_rot);
            }
        }
        private void SetLinearMotorProperties()
        {
            Vector3 dirNorm = m_lastLinearVelocityVector;

            dirNorm.Normalize();

            d.Mass objMass;
            d.BodyGetMass(Body, out objMass);
            d.Quaternion rot  = d.BodyGetQuaternion(Body);
            Quaternion   rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);

            dirNorm *= rotq;
            if (m_lMotor1 != IntPtr.Zero)
            {
                d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z);
                d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length());

                d.JointSetLMotorParam(m_lMotor1, (int)dParam.FMax, 35f * objMass.mass);
            }
        }
Exemple #4
0
        public void changeadd ()
        {
            // all prims are now created non physical
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
            m_targetSpace = targetspace;

            if (_mesh == null)
            {
                if (_parent_scene.needsMeshing (_parent_entity))
                {
                    // Don't need to re-enable body..   it's done in SetMesh
                    _mesh = _parent_scene.mesher.CreateMesh (_parent_entity.Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
                    // createmesh returns null when it's a shape that isn't a cube.
                    // m_log.Debug(m_localID);
                }
            }


                //Console.WriteLine("changeadd 1");
                CreateGeom (m_targetSpace, _mesh);

                if (prim_geom != IntPtr.Zero)
                {
                    CalculatePrimMass();

                    d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                    d.Quaternion myrot = new d.Quaternion();
                    Quaternion fake = _orientation;
                    myrot.X = fake.X;
                    myrot.Y = fake.Y;
                    myrot.Z = fake.Z;
                    myrot.W = fake.W;
                    d.GeomSetQuaternion(prim_geom, ref myrot);
                    //                    _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                }
/*
            if (m_isphysical && Body == IntPtr.Zero)
            {
                if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
                {
                    changeshape(_pbs);
                }
                else
                {
                    MakeBody();
                }
            }
*/
            changeSelectedStatus (m_isSelected);
        }
Exemple #5
0
        public void changeprimsizeshape()
        {
            _parent_scene.actor_name_map.Remove(prim_geom);
            WhiteCoreODEPrim parent = null;

            bool chp = childPrim;
            if (chp)
                parent = (WhiteCoreODEPrim) _parent;

            // Cleanup of old prim geometry and Bodies
            if (chp)
            {
                if (parent != null)
                    parent.DestroyBody();
            }
            else
            {
                DestroyBody();
            }

            if (prim_geom != IntPtr.Zero)
            {
                try
                {
                    d.GeomDestroy(prim_geom);
                }
                catch (AccessViolationException)
                {
                    prim_geom = IntPtr.Zero;
                    MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
                }
                prim_geom = IntPtr.Zero;
            }
            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0)
                _size.X = 0.01f;
            if (_size.Y <= 0)
                _size.Y = 0.01f;
            if (_size.Z <= 0)
                _size.Z = 0.01f;

            CreateGeom(m_targetSpace);

            CalcPrimBodyData();

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                Quaternion fake = _orientation;
                myrot.X = fake.X;
                myrot.Y = fake.Y;
                myrot.Z = fake.Z;
                myrot.W = fake.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);

                _parent_scene.actor_name_map[prim_geom] = this;
            }

            changeSelectedStatus(m_isSelected);

            if (chp)
            {
                if (parent != null)
                {
                    parent.MakeBody();
                }
            }
            else
                MakeBody();
        }
Exemple #6
0
        public void MakeBody ()
        {
            //            d.Vector3 dvtmp;
            //            d.Vector3 dbtmp;

            d.Mass tmpdmass = new d.Mass
            {
            };
            d.Mass objdmass = new d.Mass
            {
            };

            d.Matrix3 mat = new d.Matrix3 ();
            d.Matrix3 mymat = new d.Matrix3 ();
            d.Quaternion quat = new d.Quaternion ();
            d.Quaternion myrot = new d.Quaternion ();
            Vector3 rcm;

            if (childPrim)  // child prims don't get own bodies;
                return;

            if (Body != IntPtr.Zero) // who shouldn't have one already ?
            {
                d.BodyDestroy (Body);
                Body = IntPtr.Zero;
            }

            if (!m_isphysical) // only physical things get a body
                return;
            Body = d.BodyCreate (_parent_scene.world);

            calcdMass (); // compute inertia on local frame

            DMassDup (ref primdMass, out objdmass);

            // rotate inertia
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            myrot.W = _orientation.W;

            d.RfromQ (out mymat, ref myrot);
            d.MassRotate (ref objdmass, ref mymat);

            // set the body rotation and position
            d.BodySetRotation (Body, ref mymat);

            // recompute full object inertia if needed
            if (childrenPrim.Count > 0)
            {

                rcm.X = _position.X + objdmass.c.X;
                rcm.Y = _position.Y + objdmass.c.Y;
                rcm.Z = _position.Z + objdmass.c.Z;

                lock (childrenPrim)
                {
                    foreach (AuroraODEPrim prm in childrenPrim)
                    {
                        prm.calcdMass (); // recompute inertia on local frame
                        DMassCopy (ref prm.primdMass, ref tmpdmass);

                        // apply prim current rotation to inertia
                        quat.W = prm._orientation.W;
                        quat.X = prm._orientation.X;
                        quat.Y = prm._orientation.Y;
                        quat.Z = prm._orientation.Z;
                        d.RfromQ (out mat, ref quat);
                        d.MassRotate (ref tmpdmass, ref mat);

                        Vector3 ppos = prm._position;
                        ppos.X += tmpdmass.c.X - rcm.X;
                        ppos.Y += tmpdmass.c.Y - rcm.Y;
                        ppos.Z += tmpdmass.c.Z - rcm.Z;

                        // refer inertia to root prim center of mass position
                        d.MassTranslate (ref tmpdmass,
                            ppos.X,
                            ppos.Y,
                            ppos.Z);

                        d.MassAdd (ref objdmass, ref tmpdmass); // add to total object inertia

                        // fix prim colision cats
                        if (prm.prim_geom == IntPtr.Zero)
                        {
                            m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
                            continue;
                        }

                        d.GeomClearOffset (prm.prim_geom);
                        d.GeomSetBody (prm.prim_geom, Body);
                        d.GeomSetOffsetWorldRotation (prm.prim_geom, ref mat); // set relative rotation
                    }
                }
            }

            d.GeomClearOffset (prim_geom); // make sure we don't have a hidden offset
            // associate root geom with body
            d.GeomSetBody (prim_geom, Body);

            d.BodySetPosition (Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
            d.GeomSetOffsetWorldPosition (prim_geom, _position.X, _position.Y, _position.Z);

            d.MassTranslate (ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
            myrot.W = -myrot.W;
            d.RfromQ (out mymat, ref myrot);
            d.MassRotate (ref objdmass, ref mymat);
            d.BodySetMass (Body, ref objdmass);
            _mass = objdmass.mass;

            m_collisionCategories |= CollisionCategories.Body;
            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

            // disconnect from world gravity so we can apply buoyancy
            if (!testRealGravity)
                d.BodySetGravityMode (Body, false);

            d.BodySetAutoDisableFlag (Body, true);
            d.BodySetAutoDisableSteps (Body, body_autodisable_frames);
            //            d.BodySetLinearDampingThreshold(Body, 0.01f);
            //            d.BodySetAngularDampingThreshold(Body, 0.001f);
            d.BodySetDamping (Body, .001f, .001f);
            m_disabled = false;

            d.GeomSetCategoryBits (prim_geom, (int)m_collisionCategories);
            d.GeomSetCollideBits (prim_geom, (int)m_collisionFlags);

            m_interpenetrationcount = 0;
            m_collisionscore = 0;

            if (m_targetSpace != _parent_scene.space)
            {
                _parent_scene.waitForSpaceUnlock (m_targetSpace);
                if (d.SpaceQuery (m_targetSpace, prim_geom))
                    d.SpaceRemove (m_targetSpace, prim_geom);

                m_targetSpace = _parent_scene.space;
                d.SpaceAdd (m_targetSpace, prim_geom);
            }

            lock (childrenPrim)
            {
                foreach (AuroraODEPrim prm in childrenPrim)
                {
                    if (prm.prim_geom == IntPtr.Zero)
                    {
                        m_log.Warn ("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
                        continue;
                    }
                    Vector3 ppos = prm._position;
                    d.GeomSetOffsetWorldPosition (prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position

                    prm.m_collisionCategories |= CollisionCategories.Body;
                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
                    d.GeomSetCategoryBits (prm.prim_geom, (int)prm.m_collisionCategories);
                    d.GeomSetCollideBits (prm.prim_geom, (int)prm.m_collisionFlags);

                    prm.Body = Body;
                    prm.m_disabled = false;
                    prm.m_interpenetrationcount = 0;
                    prm.m_collisionscore = 0;
                    _parent_scene.addActivePrim (prm);

                    if (prm.m_targetSpace != _parent_scene.space)
                    {
                        _parent_scene.waitForSpaceUnlock (m_targetSpace);
                        if (d.SpaceQuery (prm.m_targetSpace, prm.prim_geom))
                            d.SpaceRemove (prm.m_targetSpace, prm.prim_geom);

                        prm.m_targetSpace = _parent_scene.space;
                        d.SpaceAdd (m_targetSpace, prm.prim_geom);
                    }
                }
            }
            // The body doesn't already have a finite rotation mode set here
            if ((!m_angularlock.ApproxEquals (Vector3.One, 0.0f)) && _parent == null)
            {
                createAMotor (m_angularlock);
            }
            if (m_vehicle.Type != Vehicle.TYPE_NONE)
            {
                m_vehicle.Enable (Body, this, _parent_scene);
            }
            _parent_scene.addActivePrim (this);

            /*            d.Mass mtmp;
                        d.BodyGetMass(Body, out mtmp);
                        d.Matrix3 mt = d.GeomGetRotation(prim_geom);
                        d.Matrix3 mt2 = d.BodyGetRotation(Body);
                        dvtmp = d.GeomGetPosition(prim_geom);
                        dbtmp = d.BodyGetPosition(Body);
            */
        }
Exemple #7
0
        /// <summary>
        /// Add a child prim to this parent prim.
        /// </summary>
        /// <param name="prim">Child prim</param>
        private void AddChildPrim(OdePrim prim)
        {
            if (LocalID == prim.LocalID)
                return;

            if (Body == IntPtr.Zero)
            {
                Body = d.BodyCreate(_parent_scene.world);
                setMass();
            }

            lock (childrenPrim)
            {
                if (childrenPrim.Contains(prim))
                    return;

//                m_log.DebugFormat(
//                    "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);

                childrenPrim.Add(prim);

                foreach (OdePrim prm in childrenPrim)
                {
                    d.Mass m2;
                    d.MassSetZero(out m2);
                    d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);

                    d.Quaternion quat = new d.Quaternion();
                    quat.W = prm._orientation.W;
                    quat.X = prm._orientation.X;
                    quat.Y = prm._orientation.Y;
                    quat.Z = prm._orientation.Z;

                    d.Matrix3 mat = new d.Matrix3();
                    d.RfromQ(out mat, ref quat);
                    d.MassRotate(ref m2, ref mat);
                    d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
                    d.MassAdd(ref pMass, ref m2);
                }

                foreach (OdePrim prm in childrenPrim)
                {
                    prm.m_collisionCategories |= CollisionCategories.Body;
                    prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
                    if (prm.m_assetFailed)
                    {
                        d.GeomSetCategoryBits(prm.prim_geom, 0);
                        d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits);
                    }
                    else
                    {
                        d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                        d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
                    }

                    d.Quaternion quat = new d.Quaternion();
                    quat.W = prm._orientation.W;
                    quat.X = prm._orientation.X;
                    quat.Y = prm._orientation.Y;
                    quat.Z = prm._orientation.Z;

                    d.Matrix3 mat = new d.Matrix3();
                    d.RfromQ(out mat, ref quat);
                    if (Body != IntPtr.Zero)
                    {
                        d.GeomSetBody(prm.prim_geom, Body);
                        prm.childPrim = true;
                        d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
                        //d.GeomSetOffsetPosition(prim.prim_geom,
                        //    (Position.X - prm.Position.X) - pMass.c.X,
                        //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                        //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                        d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
                        //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
                        d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                        d.BodySetMass(Body, ref pMass);
                    }
                    else
                    {
                        m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
                    }

                    prm.m_interpenetrationcount = 0;
                    prm.m_collisionscore = 0;
                    prm.m_disabled = false;

                    // The body doesn't already have a finite rotation mode set here
                    if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
                    {
                        prm.createAMotor(m_angularlock);
                    }
                    prm.Body = Body;
                    _parent_scene.ActivatePrim(prm);
                }

                m_collisionCategories |= CollisionCategories.Body;
                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                if (m_assetFailed)
                {
                    d.GeomSetCategoryBits(prim_geom, 0);
                    d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits);
                }
                else
                {
                    //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
                    d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                    //Console.WriteLine(" Post GeomSetCategoryBits 2");
                    d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
                }

                d.Quaternion quat2 = new d.Quaternion();
                quat2.W = _orientation.W;
                quat2.X = _orientation.X;
                quat2.Y = _orientation.Y;
                quat2.Z = _orientation.Z;

                d.Matrix3 mat2 = new d.Matrix3();
                d.RfromQ(out mat2, ref quat2);
                d.GeomSetBody(prim_geom, Body);
                d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
                //d.GeomSetOffsetPosition(prim.prim_geom,
                //    (Position.X - prm.Position.X) - pMass.c.X,
                //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                //d.GeomSetOffsetRotation(prim_geom, ref mat2);
                d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                d.BodySetMass(Body, ref pMass);

                d.BodySetAutoDisableFlag(Body, true);
                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);

                m_interpenetrationcount = 0;
                m_collisionscore = 0;
                m_disabled = false;

                // The body doesn't already have a finite rotation mode set here
                if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
                {
                    createAMotor(m_angularlock);
                }

                d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);

                if (m_vehicle.Type != Vehicle.TYPE_NONE)
                    m_vehicle.Enable(Body, _parent_scene);

                _parent_scene.ActivatePrim(this);
            }
        }
Exemple #8
0
        public void changeshape(float timestamp)
        {
            string oldname = _parent_scene.geom_name_map[prim_geom];

            // Cleanup of old prim geometry and Bodies
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        AuroraODEPrim parent = (AuroraODEPrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }
            try
            {
                d.GeomDestroy(prim_geom);
            }
            catch (System.AccessViolationException)
            {
                prim_geom = IntPtr.Zero;
                m_log.Error("[PHYSICS]: PrimGeom dead");
            }
            prim_geom = IntPtr.Zero;
            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;
            // Construction of new prim

            if (_parent_scene.needsMeshing(_pbs))
            {
                // Don't need to re-enable body..   it's done in SetMesh
                float meshlod = _parent_scene.meshSculptLOD;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;

                IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
                // createmesh returns null when it doesn't mesh.
                CreateGeom(m_targetSpace, mesh);
            }
            else
            {
                _mesh = null;
//Console.WriteLine("changeshape");
                CreateGeom(m_targetSpace, null);
            }

            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            //myrot.W = _orientation.w;
            myrot.W = _orientation.W;
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
            if (IsPhysical && Body == IntPtr.Zero)
            {
                // Re creates body on size.
                // EnableBody also does setMass()
                enableBody();
                if (Body != IntPtr.Zero)
                {
                    d.BodyEnable(Body);
                }
            }
            _parent_scene.geom_name_map[prim_geom] = oldname;

            changeSelectedStatus(timestamp);
            if (childPrim)
            {
                if (_parent is AuroraODEPrim)
                {
                    AuroraODEPrim parent = (AuroraODEPrim)_parent;
                    parent.ChildSetGeom(this);
                }
            }
            resetCollisionAccounting();
            m_taintshape = false;
        }
Exemple #9
0
 private d.Quaternion ConvertTodQuat(Quaternion q)
 {
     d.Quaternion dq = new d.Quaternion { X = q.X, Y = q.Y, Z = q.Z, W = q.W };
     return dq;
 }
Exemple #10
0
 public void rotate(float timestep)
 {
     d.Quaternion myrot = new d.Quaternion();
     myrot.X = _orientation.X;
     myrot.Y = _orientation.Y;
     myrot.Z = _orientation.Z;
     myrot.W = _orientation.W;
     if (Body != IntPtr.Zero)
     {
         // KF: If this is a root prim do BodySet
         d.BodySetQuaternion(Body, ref myrot);
         if (m_isphysical)
         {
             if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
                 createAMotor(m_angularlock);
         }
     }
     else
     {
         // daughter prim, do Geom set
         d.GeomSetQuaternion(prim_geom, ref myrot);
     }
     
     resetCollisionAccounting();
     m_taintrot = _orientation;
 }
        public void ProcessTaints(float timestep)
        {
            if (!m_shouldBePhysical)
                return;

            if (m_tainted_isPhysical != m_isPhysical)
            {
                if (m_tainted_isPhysical)
                {
                    // Create avatar capsule and related ODE data
                    if (!(Shell == IntPtr.Zero && Body == IntPtr.Zero)) // && Amotor == IntPtr.Zero))
                    {
                        m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - "
                            + (Shell!=IntPtr.Zero ? "Shell ":"")
                            + (Body!=IntPtr.Zero ? "Body ":"")
                            );
                            // + (Amotor!=IntPtr.Zero ? "Amotor ":""));
                    }
                    AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z);//, _parent_scene.avStandupTensor);
                    
                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
                    _parent_scene.AddCharacter(this);
                }
                else
                {
                    _parent_scene.RemoveCharacter(this);
                    // destroy avatar capsule and related ODE data
                    if (Amotor != IntPtr.Zero)
                    {
                        // Kill the Amotor
                        d.JointDestroy (Amotor);
                        Amotor = IntPtr.Zero;
                    }
                    //kill the Geometry
                    _parent_scene.waitForSpaceUnlock(_parent_scene.space);

                    if (Body != IntPtr.Zero)
                    {
                        //kill the body
                        d.BodyDestroy(Body);

                        Body = IntPtr.Zero;
                    }

                    if (Shell != IntPtr.Zero)
                    {
                        d.GeomDestroy(Shell);
                        Shell = IntPtr.Zero;
                    }

                }

                m_isPhysical = m_tainted_isPhysical;
            }

            if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH)
            {
                if (Shell != IntPtr.Zero && Body != IntPtr.Zero) // && Amotor != IntPtr.Zero)
                {
                    if (Amotor != IntPtr.Zero)
                    {
                        // Kill the Amotor
                        d.JointDestroy (Amotor);
                        Amotor = IntPtr.Zero;
                    }
                    m_pidControllerActive = true;
                    // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate()
                    float prevCapsule = CAPSULE_LENGTH;
                    CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH;
                    //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString());
                    d.BodyDestroy(Body);
                    d.GeomDestroy(Shell);
                    AvatarGeomAndBodyCreation(_position.X, _position.Y,
                                      _position.Z + (CAPSULE_LENGTH - prevCapsule));//, _parent_scene.avStandupTensor);
                    Velocity = Vector3.Zero;

                    _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
                }
                else
                {
                    m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " 
                        + (Shell==IntPtr.Zero ? "Shell ":"")
                        + (Body==IntPtr.Zero ? "Body ":"")
                        );
                }
            }

            if (!m_taintPosition.ApproxEquals(_position, 0.05f))
            {
                if (Body != IntPtr.Zero)
                {
                    d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z);

                    _position.X = m_taintPosition.X;
                    _position.Y = m_taintPosition.Y;
                    _position.Z = m_taintPosition.Z;
                }
            }
            if (Body != IntPtr.Zero)
            {
                d.Quaternion q = new d.Quaternion ();
                q.W = m_taintRotation.W;
                q.X = m_taintRotation.X;
                q.Y = m_taintRotation.Y;
                q.Z = m_taintRotation.Z;
                d.BodySetQuaternion (Body, ref q); // just keep in sync with rest of simutator
            }
        }
Exemple #12
0
        public void changeadd()
        {
            // all prims are now created non physical
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
            m_targetSpace = targetspace;

            if (_mesh == null)
            {
                if (_parent_scene.needsMeshing(_parent_entity))
                {
                    // Don't need to re-enable body..   it's done in SetMesh
                    _mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size,
                                                            _parent_scene.meshSculptLOD, true);

                    //Tell things above if they want to cache it or something
                    if(_mesh != null)
                        _parent_entity.ParentEntity.GeneratedMesh(_parent_entity, _mesh);
                    // createmesh returns null when it's a shape that isn't a cube.
                    // MainConsole.Instance.Debug(m_localID);

                    // Remove the reference to any JPEG2000 sculpt data so it can be GCed
                    _pbs.SculptData = null;
                }
            }


            //Console.WriteLine("changeadd 1");
            CreateGeom(m_targetSpace, _mesh);
            CalcPrimBodyData();

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                Quaternion fake = _orientation;
                myrot.X = fake.X;
                myrot.Y = fake.Y;
                myrot.Z = fake.Z;
                myrot.W = fake.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);
                //                    _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
            }

            changeSelectedStatus(m_isSelected);
        }
Exemple #13
0
        /// <summary>
        /// Change prim in response to a shape taint.
        /// </summary>
        private void changeshape()
        {
            m_taintshape = false;

            // Cleanup of old prim geometry and Bodies
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        OdePrim parent = (OdePrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }

            RemoveGeom();

            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;
            // Construction of new prim

            IMesh mesh = null;


            if (_parent_scene.needsMeshing(_pbs))
            {
                // Don't need to re-enable body..   it's done in CreateMesh
                float meshlod = _parent_scene.meshSculptLOD;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;

                // createmesh returns null when it doesn't mesh.
                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
                if (mesh == null)
                    CheckMeshAsset();
                else
                    m_assetFailed = false;
            }

            CreateGeom(m_targetSpace, mesh);
            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            //myrot.W = _orientation.w;
            myrot.W = _orientation.W;
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
            if (IsPhysical && Body == IntPtr.Zero)
            {
                // Re creates body on size.
                // EnableBody also does setMass()
                enableBody();
                if (Body != IntPtr.Zero)
                {
                    d.BodyEnable(Body);
                }
            }

            changeSelectedStatus();

            if (childPrim)
            {
                if (_parent is OdePrim)
                {
                    OdePrim parent = (OdePrim)_parent;
                    parent.ChildSetGeom(this);
                }
            }

            resetCollisionAccounting();
//            m_taintshape = false;
        }
Exemple #14
0
        /// <summary>
        /// Change prim in response to a size taint.
        /// </summary>
        private void changesize()
        {
#if SPAM
            m_log.DebugFormat("[ODE PRIM]: Called changesize");
#endif

            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;

            //kill body to rebuild
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        OdePrim parent = (OdePrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }

            if (d.SpaceQuery(m_targetSpace, prim_geom))
            {
//                _parent_scene.waitForSpaceUnlock(m_targetSpace);
                d.SpaceRemove(m_targetSpace, prim_geom);
            }

            RemoveGeom();

            // we don't need to do space calculation because the client sends a position update also.

            IMesh mesh = null;

            // Construction of new prim
            if (_parent_scene.needsMeshing(_pbs))
            {
                float meshlod = _parent_scene.meshSculptLOD;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;
                // Don't need to re-enable body..   it's done in SetMesh

                if (_parent_scene.needsMeshing(_pbs))
                {
                    mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
                    if (mesh == null)
                        CheckMeshAsset();
                    else
                        m_assetFailed = false;
                }
                    
            }

            CreateGeom(m_targetSpace, mesh);
            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            myrot.W = _orientation.W;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
            if (IsPhysical && Body == IntPtr.Zero && !childPrim)
            {
                // Re creates body on size.
                // EnableBody also does setMass()
                enableBody();
                d.BodyEnable(Body);
            }

            changeSelectedStatus();

            if (childPrim)
            {
                if (_parent is OdePrim)
                {
                    OdePrim parent = (OdePrim)_parent;
                    parent.ChildSetGeom(this);
                }
            }
            resetCollisionAccounting();
            m_taintsize = _size;
        }
Exemple #15
0
        /// <summary>
        /// Add prim in response to an add taint.
        /// </summary>
        private void changeadd()
        {
//            m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
            
            int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);

            if (targetspace == IntPtr.Zero)
                targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);

            m_targetSpace = targetspace;

            IMesh mesh = null;

            if (_parent_scene.needsMeshing(_pbs))
            {
                // Don't need to re-enable body..   it's done in SetMesh
                mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
                // createmesh returns null when it's a shape that isn't a cube.
               // m_log.Debug(m_localID);
                if (mesh == null)
                    CheckMeshAsset();
                else
                    m_assetFailed = false;
            }

#if SPAM
Console.WriteLine("changeadd 1");
#endif
            CreateGeom(m_targetSpace, mesh);

            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            myrot.W = _orientation.W;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            if (IsPhysical && Body == IntPtr.Zero)
                enableBody();

            changeSelectedStatus();

            m_taintadd = false;
        }
Exemple #16
0
        public void changeOrientation(Quaternion newrot)
        {
            if (m_isphysical)
            {
                if (childPrim)
                {
                    if (m_blockPhysicalReconstruction)  // inertia is messed, must rebuild
                    {
                        _orientation = newrot;
                    }
                }
                else
                {
                    if (newrot != _orientation)
                    {
                        d.Quaternion myrot = new d.Quaternion();
                        Quaternion fake = newrot;
                        myrot.X = fake.X;
                        myrot.Y = fake.Y;
                        myrot.Z = fake.Z;
                        myrot.W = fake.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        _orientation = newrot;
                        if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
                            createAMotor(m_angularlock);
                    }
                }
                if (Body != IntPtr.Zero)
                    d.BodyEnable(Body);
            }

            else
            {
                if (newrot != _orientation)
                {
                    d.Quaternion myrot = new d.Quaternion();
                    Quaternion fake = newrot;
                    myrot.X = fake.X;
                    myrot.Y = fake.Y;
                    myrot.Z = fake.Z;
                    myrot.W = fake.W;
                    d.GeomSetQuaternion(prim_geom, ref myrot);
                    _orientation = newrot;
                }
            }
            if (--fakeori < 0)
                fakeori = 0;

            changeSelectedStatus(m_isSelected);
            resetCollisionAccounting();
        }
Exemple #17
0
        public void changeprimsizeshape()
        {
            _parent_scene.actor_name_map.Remove(prim_geom);
            AuroraODEPrim parent = null;

            bool chp = childPrim;
            if (chp)
                parent = (AuroraODEPrim)_parent;

            // Cleanup of old prim geometry and Bodies
            if (chp)
            {
                if (parent != null)
                    parent.DestroyBody();
            }
            else
            {
                DestroyBody();
            }

            if (prim_geom != IntPtr.Zero)
            {
                try
                {
                    d.GeomDestroy(prim_geom);
                }
                catch (AccessViolationException)
                {
                    prim_geom = IntPtr.Zero;
                    MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
                }
                prim_geom = IntPtr.Zero;
            }
            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0)
                _size.X = 0.01f;
            if (_size.Y <= 0)
                _size.Y = 0.01f;
            if (_size.Z <= 0)
                _size.Z = 0.01f;
            // Construction of new prim


            if (_parent_scene.needsMeshing(_parent_entity))
            {
                float meshlod = _parent_scene.meshSculptLOD;
                IMesh mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size, meshlod, true);
                // createmesh returns null when it doesn't mesh.
                CreateGeom(m_targetSpace, mesh);
            }
            else
            {
                _mesh = null;
                //Console.WriteLine("changeshape");
                CreateGeom(m_targetSpace, null);
            }

            CalcPrimBodyData();

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                Quaternion fake = _orientation;
                myrot.X = fake.X;
                myrot.Y = fake.Y;
                myrot.Z = fake.Z;
                myrot.W = fake.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);

                _parent_scene.actor_name_map[prim_geom] = this;
            }

            changeSelectedStatus(m_isSelected);

            if (chp)
            {
                if (parent != null)
                {
                    parent.MakeBody();
                }
            }
            else
                MakeBody();
            ;
        }
Exemple #18
0
        // I'm the parent
        // prim is the child
        public void ParentPrim(AuroraODEPrim prim)
        {
//Console.WriteLine("ParentPrim  " + m_primName);
            if (this.m_localID != prim.m_localID)
            {
                if (Body == IntPtr.Zero)
                {
                    Body = d.BodyCreate(_parent_scene.world);
                    setMass();
                }
                if (Body != IntPtr.Zero)
                {
                    lock (childrenPrim)
                    {
                        if (!childrenPrim.Contains(prim))
                        {
//Console.WriteLine("childrenPrim.Add " + prim);
                            childrenPrim.Add(prim);
                            
                            foreach (AuroraODEPrim prm in childrenPrim)
                            {
                                d.Mass m2;
                                d.MassSetZero(out m2);
                                prim.Mass = prim.CalculateMass();
                                d.MassSetBoxTotal(out m2, prim.Mass, prm._size.X, prm._size.Y, prm._size.Z);


                                d.Quaternion quat = new d.Quaternion();
                                quat.W = prm._orientation.W;
                                quat.X = prm._orientation.X;
                                quat.Y = prm._orientation.Y;
                                quat.Z = prm._orientation.Z;

                                d.Matrix3 mat = new d.Matrix3();
                                d.RfromQ(out mat, ref quat);
                                d.MassRotate(ref m2, ref mat);
                                d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
                                d.MassAdd(ref pMass, ref m2);
                            }
                            foreach (AuroraODEPrim prm in childrenPrim)
                            {
                       
                                prm.m_collisionCategories |= CollisionCategories.Body;
                                prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                                if (prm.prim_geom == IntPtr.Zero)
                                {
                                    m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements.  No geom yet");
                                    continue;
                                }
//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
                                d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
                                d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);


                                d.Quaternion quat = new d.Quaternion();
                                quat.W = prm._orientation.W;
                                quat.X = prm._orientation.X;
                                quat.Y = prm._orientation.Y;
                                quat.Z = prm._orientation.Z;

                                d.Matrix3 mat = new d.Matrix3();
                                d.RfromQ(out mat, ref quat);
                                if (Body != IntPtr.Zero)
                                {
                                    d.GeomSetBody(prm.prim_geom, Body);
                                    prm.childPrim = true;
                                    d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
                                    //d.GeomSetOffsetPosition(prim.prim_geom,
                                    //    (Position.X - prm.Position.X) - pMass.c.X,
                                    //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                                    //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                                    d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
                                    //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
                                    d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                                    d.BodySetMass(Body, ref pMass);
                                }
                                else
                                {
                                    m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body");
                                }


                                prm.m_interpenetrationcount = 0;
                                prm.m_collisionscore = 0;
                                prm.m_disabled = false;

                                // The body doesn't already have a finite rotation mode set here
                                if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
                                {
                                    prm.createAMotor(m_angularlock);
                                }
                                prm.Body = Body;
                                _parent_scene.addActivePrim(prm);
                            }
                            m_collisionCategories |= CollisionCategories.Body;
                            m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
                            d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
//Console.WriteLine(" Post GeomSetCategoryBits 2");
                            d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);


                            d.Quaternion quat2 = new d.Quaternion();
                            quat2.W = _orientation.W;
                            quat2.X = _orientation.X;
                            quat2.Y = _orientation.Y;
                            quat2.Z = _orientation.Z;

                            d.Matrix3 mat2 = new d.Matrix3();
                            d.RfromQ(out mat2, ref quat2);
                            d.GeomSetBody(prim_geom, Body);
                            d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
                            //d.GeomSetOffsetPosition(prim.prim_geom,
                            //    (Position.X - prm.Position.X) - pMass.c.X,
                            //    (Position.Y - prm.Position.Y) - pMass.c.Y,
                            //    (Position.Z - prm.Position.Z) - pMass.c.Z);
                            //d.GeomSetOffsetRotation(prim_geom, ref mat2);
                            d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
                            d.BodySetMass(Body, ref pMass);

                            d.BodySetAutoDisableFlag(Body, true);
                            d.BodySetAutoDisableSteps(Body, body_autodisable_frames);


                            m_interpenetrationcount = 0;
                            m_collisionscore = 0;
                            m_disabled = false;

                            // The body doesn't already have a finite rotation mode set here
                            if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null)
                            {
                                createAMotor(m_angularlock);
                            }
                            d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
                            if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, this, _parent_scene);
                            _parent_scene.addActivePrim(this);
                        }
                    }
                }
            }

        }
Exemple #19
0
 } //end MoveAngular
 internal void LimitRotation(float timestep)
 {
     d.Quaternion rot = d.BodyGetQuaternion(Body);
     Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
     d.Quaternion m_rot = new d.Quaternion();
     bool changed = false;
     m_rot.X = rotq.X;
     m_rot.Y = rotq.Y;
     m_rot.Z = rotq.Z;
     m_rot.W = rotq.W;
     if (m_RollreferenceFrame != Quaternion.Identity)
     {
         if (rotq.X >= m_RollreferenceFrame.X)
         {
             m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
         }
         if (rotq.Y >= m_RollreferenceFrame.Y)
         {
             m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
         }
         if (rotq.X <= -m_RollreferenceFrame.X)
         {
             m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
         }
         if (rotq.Y <= -m_RollreferenceFrame.Y)
         {
             m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
         }
         changed = true;
     }
     if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
     {
         m_rot.X = 0;
         m_rot.Y = 0;
         changed = true;
     }
     if (changed)
         d.BodySetQuaternion(Body, ref m_rot);
 }
Exemple #20
0
        public void changesize(float timestamp)
        {
            
            string oldname = _parent_scene.geom_name_map[prim_geom];

            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;

            // Cleanup of old prim geometry
            if (_mesh != null)
            {
                // Cleanup meshing here
            }
            //kill body to rebuild
            if (IsPhysical && Body != IntPtr.Zero)
            {
                if (childPrim)
                {
                    if (_parent != null)
                    {
                        AuroraODEPrim parent = (AuroraODEPrim)_parent;
                        parent.ChildDelink(this);
                    }
                }
                else
                {
                    disableBody();
                }
            }
            if (d.SpaceQuery(m_targetSpace, prim_geom))
            {
                _parent_scene.waitForSpaceUnlock(m_targetSpace);
                d.SpaceRemove(m_targetSpace, prim_geom);
            }
            d.GeomDestroy(prim_geom);
            prim_geom = IntPtr.Zero;
            // we don't need to do space calculation because the client sends a position update also.

            // Construction of new prim
            if (_parent_scene.needsMeshing(_pbs))
            {
                float meshlod = _parent_scene.meshSculptLOD;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;
                // Don't need to re-enable body..   it's done in SetMesh

                IMesh mesh = null;

                if (_parent_scene.needsMeshing(_pbs))
                    mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);

                //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
//Console.WriteLine("changesize 1");
                CreateGeom(m_targetSpace, mesh);

               
            }
            else
            {
                _mesh = null;
//Console.WriteLine("changesize 2");
                CreateGeom(m_targetSpace, _mesh);
            }

            d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
            d.Quaternion myrot = new d.Quaternion();
            myrot.X = _orientation.X;
            myrot.Y = _orientation.Y;
            myrot.Z = _orientation.Z;
            myrot.W = _orientation.W;
            d.GeomSetQuaternion(prim_geom, ref myrot);

            //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
            if (IsPhysical && Body == IntPtr.Zero && !childPrim)
            {
                // Re creates body on size.
                // EnableBody also does setMass()
                enableBody();
                d.BodyEnable(Body);
            }

            _parent_scene.geom_name_map[prim_geom] = oldname;

            changeSelectedStatus(timestamp);
            if (childPrim)
            {
                if (_parent is AuroraODEPrim)
                {
                    AuroraODEPrim parent = (AuroraODEPrim)_parent;
                    parent.ChildSetGeom(this);
                }
            }
            resetCollisionAccounting();
            m_taintsize = _size;
        }
Exemple #21
0
        public void changeadd()
            {
//            int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);

//            if (targetspace == IntPtr.Zero)
//                targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);

            m_targetSpace = targetspace;

            if (_mesh == null)
            {
                if (_parent_scene.needsMeshing(_pbs))
                {
                    // Don't need to re-enable body..   it's done in SetMesh
                    _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
                    // createmesh returns null when it's a shape that isn't a cube.
                    // m_log.Debug(m_localID);
                }
            }


            lock (_parent_scene.OdeLock)
                {
                //Console.WriteLine("changeadd 1");
                CreateGeom(m_targetSpace, _mesh);

                if (prim_geom != IntPtr.Zero)
                    {

                    CalculatePrimMass();
/*
                    if (m_isphysical)
                        MakeBody();

                    else
*/
                        {                       
                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                        d.Quaternion myrot = new d.Quaternion();
                        myrot.X = _orientation.X;
                        myrot.Y = _orientation.Y;
                        myrot.Z = _orientation.Z;
                        myrot.W = _orientation.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        }
                    _parent_scene.geom_name_map[prim_geom] = this.m_primName;
                    _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                    }
                }
            changeSelectedStatus(m_isSelected);
            }
Exemple #22
0
        public void enableBody()
        {
            // Don't enable this body if we're a child prim
            // this should be taken care of in the parent function not here
            if (!childPrim)
            {
                // Sets the geom to a body
                Body = d.BodyCreate(_parent_scene.world);

                setMass();
                d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                myrot.X = _orientation.X;
                myrot.Y = _orientation.Y;
                myrot.Z = _orientation.Z;
                myrot.W = _orientation.W;
                d.BodySetQuaternion(Body, ref myrot);
                d.GeomSetBody(prim_geom, Body);
                m_collisionCategories |= CollisionCategories.Body;
                m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);

                d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
                d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);

                d.BodySetAutoDisableFlag(Body, true);
                d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
                d.BodySetDamping(Body, 1, 1);
                
                // disconnect from world gravity so we can apply buoyancy
                if(!testRealGravity)
                    d.BodySetGravityMode(Body, false);

                m_interpenetrationcount = 0;
                m_collisionscore = 0;
                m_disabled = false;

                // The body doesn't already have a finite rotation mode set here
                if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
                {
                    createAMotor(m_angularlock);
                }
                if (m_vehicle.Type != Vehicle.TYPE_NONE)
                {
                    m_vehicle.Enable(Body, this, _parent_scene);
                }

                _parent_scene.addActivePrim(this);
            }
        }
Exemple #23
0
 private d.Quaternion ConvertTodQuat(Quaternion q)
 {
     d.Quaternion dq = new d.Quaternion();
     dq.X = q.X;
     dq.Y = q.Y;
     dq.Z = q.Z;
     dq.W = q.W;
     return dq;
 }
Exemple #24
0
 private void UpdateDataFromGeom()
     {
     if (prim_geom != IntPtr.Zero)
         {
         d.Vector3 lpos = d.GeomGetPosition(prim_geom);
         _position.X = lpos.X;
         _position.Y = lpos.Y;
         _position.Z = lpos.Z;
         d.Quaternion qtmp = new d.Quaternion { };
         d.GeomCopyQuaternion(prim_geom, out qtmp);
         _orientation.W = qtmp.W;
         _orientation.X = qtmp.X;
         _orientation.Y = qtmp.Y;
         _orientation.Z = qtmp.Z;
         }
     }
 public void SetRotationLocked(Quaternion taintRot)
 {
     d.Quaternion q = new d.Quaternion
     {
         W = taintRot.W,
         X = taintRot.X,
         Y = taintRot.Y,
         Z = taintRot.Z
     };
     d.BodySetQuaternion(Body, ref q); // just keep in sync with rest of simutator
 }
Exemple #26
0
        public void changemoveandrotate(Vector3 newpos,Quaternion newrot)
            {
            if (IsPhysical)
                {
                if (childPrim)  // inertia is messed, must rebuild
                    {
                    AuroraODEPrim parent = (AuroraODEPrim)_parent;
                    parent.DestroyBody();
                    parent.MakeBody();
                    }
                else
                    {
                    if (newrot != _orientation)
                        {
                        d.Quaternion myrot = new d.Quaternion();
                        myrot.X = newrot.X;
                        myrot.Y = newrot.Y;
                        myrot.Z = newrot.Z;
                        myrot.W = newrot.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
                            createAMotor(m_angularlock);
                        }
                    d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
                    }
                if (Body != IntPtr.Zero)
                    d.BodyEnable(Body);
                }

            else
                {
                // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
                // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
                _parent_scene.waitForSpaceUnlock(m_targetSpace);

                IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, newpos, m_targetSpace);
                m_targetSpace = tempspace;

                _parent_scene.waitForSpaceUnlock(m_targetSpace);

                if (newrot != _orientation)
                    {
                    d.Quaternion myrot = new d.Quaternion();
                    myrot.X = newrot.X;
                    myrot.Y = newrot.Y;
                    myrot.Z = newrot.Z;
                    myrot.W = newrot.W;
                    d.GeomSetQuaternion(prim_geom, ref myrot);
                    }
                d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);

                _parent_scene.waitForSpaceUnlock(m_targetSpace);
                d.SpaceAdd(m_targetSpace, prim_geom);
                }

            _orientation=newrot;
            _position = newpos;
            fakepos = false;
            fakeori = false;
            changeSelectedStatus(m_isSelected);

            resetCollisionAccounting();
            }
Exemple #27
0
        }// end Step

        private void MoveAngular(float pTimestep)
        {
            /*
             * private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
             * private int m_angularMotorApply = 0;                            // application frame counter
             * private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
             * private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
             * private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
             * private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
             * private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
             */

            // Get what the body is doing, this includes 'external' influences
            d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
            //         Vector3 angularVelocity = Vector3.Zero;

            if (m_angularMotorApply > 0)
            {
                // ramp up to new value
                //   current velocity  +=                         error                       /    (time to get there / step interval)
                //                               requested speed            -  last motor speed
                m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);

                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                // velocity may still be acheived.
            }
            else
            {
                // no motor recently applied, keep the body velocity

                /*        m_angularMotorVelocity.X = angularVelocity.X;
                 *      m_angularMotorVelocity.Y = angularVelocity.Y;
                 *      m_angularMotorVelocity.Z = angularVelocity.Z; */

                // and decay the velocity
                m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
            } // end motor section

            // Vertical attractor section
            Vector3 vertattr = Vector3.Zero;

            if (m_verticalAttractionTimescale < 300)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
                // get present body rotation
                d.Quaternion rot  = d.BodyGetQuaternion(Body);
                Quaternion   rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
                // make a vector pointing up
                Vector3 verterr = Vector3.Zero;
                verterr.Z = 1.0f;
                // rotate it to Body Angle
                verterr = verterr * rotq;
                // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
                // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
                // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
                if (verterr.Z < 0.0f)
                {
                    verterr.X = 2.0f - verterr.X;
                    verterr.Y = 2.0f - verterr.Y;
                }
                // Error is 0 (no error) to +/- 2 (max error)
                // scale it by VAservo
                verterr = verterr * VAservo;
                //if (frcount == 0) Console.WriteLine("VAerr=" + verterr);

                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
                vertattr.X = verterr.Y;
                vertattr.Y = -verterr.X;
                vertattr.Z = 0f;

                // scaling appears better usingsquare-law
                float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
                vertattr.X += bounce * angularVelocity.X;
                vertattr.Y += bounce * angularVelocity.Y;
            } // else vertical attractor is off

            //        m_lastVertAttractor = vertattr;

            // Bank section tba
            // Deflection section tba

            // Sum velocities
            m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection

            if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
            }

            if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
                if (!d.BodyIsEnabled(Body))
                {
                    d.BodyEnable(Body);
                }
            }
            else
            {
                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
            }

            // apply friction
            Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);

            m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;

            // Apply to the body
            d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
        }
Exemple #28
0
        public void changeprimsizeshape()
            {

            _parent_scene.geom_name_map.Remove(prim_geom);
            _parent_scene.actor_name_map.Remove(prim_geom);

            bool chp = childPrim;
            // Cleanup of old prim geometry and Bodies
            if (IsPhysical && Body != IntPtr.Zero)
                {
                if (chp)
                    {
                    if (_parent != null)
                        {
                        AuroraODEPrim parent = (AuroraODEPrim)_parent;
                        parent.ChildDelink(this);
                        }
                    }
                else
                    {
                    DestroyBody();
                    }
                }
            if (prim_geom != IntPtr.Zero)
                {
                try
                    {
                    d.GeomDestroy(prim_geom);
                    }
                catch (System.AccessViolationException)
                    {
                    prim_geom = IntPtr.Zero;
                    m_log.Error("[PHYSICS]: PrimGeom dead");
                    }
                prim_geom = IntPtr.Zero;
                }
            // we don't need to do space calculation because the client sends a position update also.
            if (_size.X <= 0) _size.X = 0.01f;
            if (_size.Y <= 0) _size.Y = 0.01f;
            if (_size.Z <= 0) _size.Z = 0.01f;
            // Construction of new prim

            if (_parent_scene.needsMeshing(_pbs))
                {
                // Don't need to re-enable body..   it's done in SetMesh
                float meshlod = _parent_scene.meshSculptLOD;

                if (IsPhysical)
                    meshlod = _parent_scene.MeshSculptphysicalLOD;

                IMesh mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, meshlod, IsPhysical);
                // createmesh returns null when it doesn't mesh.
                CreateGeom(m_targetSpace, mesh);
                }
            else
                {
                _mesh = null;
                //Console.WriteLine("changeshape");
                CreateGeom(m_targetSpace, null);
                }

            lock (_parent_scene.OdeLock)
                {
                if (prim_geom != IntPtr.Zero)
                    {
                    CalculatePrimMass();

                    if (m_isphysical && !chp)
                        MakeBody();
                    else
                        {
                        d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                        d.Quaternion myrot = new d.Quaternion();
                        myrot.X = _orientation.X;
                        myrot.Y = _orientation.Y;
                        myrot.Z = _orientation.Z;
                        myrot.W = _orientation.W;
                        d.GeomSetQuaternion(prim_geom, ref myrot);
                        }

                    _parent_scene.geom_name_map[prim_geom] = this.m_primName;
                    _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
                    }
                }

            changeSelectedStatus(m_isSelected);

            if (chp)
                {
                if (_parent is AuroraODEPrim)
                    {
                    AuroraODEPrim parent = (AuroraODEPrim)_parent;
                    parent.ChildSetGeom(this);
                    }
                }
            resetCollisionAccounting();
            }
Exemple #29
0
        private void MoveLinear(float pTimestep, OdeScene _pParentScene)
        {
            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
            {
                if (!d.BodyIsEnabled(Body))
                {
                    d.BodyEnable(Body);
                }

                // add drive to body
                Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / pTimestep);
                m_lastLinearVelocityVector += (addAmount * 10);  // lastLinearVelocityVector is the current body velocity vector?

                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                {
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
                {
                    m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
                }
                if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
                {
                    m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
                }

                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One / (m_linearMotorDecayTimescale / pTimestep)));
                //Console.WriteLine("decay: " + decayfraction);
                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
                //Console.WriteLine("actual: " + m_linearMotorDirection);
            }
            else
            {        // requested is not significant
                // if what remains of applied is small, zero it.
                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
                {
                    m_lastLinearVelocityVector = Vector3.Zero;
                }
            }

            // convert requested object velocity to world-referenced vector
            m_dir = m_lastLinearVelocityVector;
            d.Quaternion rot  = d.BodyGetQuaternion(Body);
            Quaternion   rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object

            m_dir *= rotq;                                                  // apply obj rotation to velocity vector

            // add Gravity andBuoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;

            // There is some gravity, make a gravity force vector
            // that is applied after object velocity.
            d.Mass objMass;
            d.BodyGetMass(Body, out objMass);
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
            grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
            // Preserve the current Z velocity
            d.Vector3 vel_now = d.BodyGetLinearVel(Body);
            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity

            d.Vector3 pos = d.BodyGetPosition(Body);
            //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
            Vector3 posChange = new Vector3();

            posChange.X = pos.X - m_lastPositionVector.X;
            posChange.Y = pos.Y - m_lastPositionVector.Y;
            posChange.Z = pos.Z - m_lastPositionVector.Z;
            double Zchange = Math.Abs(posChange.Z);

            if (m_BlockingEndPoint != Vector3.Zero)
            {
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= posChange.Y + 1;
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= posChange.Z + 1;
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.X <= 0)
                {
                    pos.X += posChange.X + 1;
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
                if (pos.Y <= 0)
                {
                    pos.Y += posChange.Y + 1;
                    d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
                }
            }
            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
            {
                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
                d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
            }

            // Check if hovering
            if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
            {
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
                    m_VhoverTargetHeight = m_VhoverHeight;
                }

                if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
                {
                    // If body is aready heigher, use its height as target height
                    if (pos.Z > m_VhoverTargetHeight)
                    {
                        m_VhoverTargetHeight = pos.Z;
                    }
                }
                if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                {
                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
                    {
                        d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
                    }
                }
                else
                {
                    float herr0 = pos.Z - m_VhoverTargetHeight;
                    // Replace Vertical speed with correction figure if significant
                    if (Math.Abs(herr0) > 0.01f)
                    {
                        m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
                        //KF: m_VhoverEfficiency is not yet implemented
                    }
                    else
                    {
                        m_dir.Z = 0f;
                    }
                }

                //                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
                //                m_VhoverTimescale = 0f;        // time to acheive height
                //                pTimestep  is time since last frame,in secs
            }

            if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
            {
                //Start Experimental Values
                if (Zchange > .3)
                {
                    grav.Z = (float)(grav.Z * 3);
                }
                if (Zchange > .15)
                {
                    grav.Z = (float)(grav.Z * 2);
                }
                if (Zchange > .75)
                {
                    grav.Z = (float)(grav.Z * 1.5);
                }
                if (Zchange > .05)
                {
                    grav.Z = (float)(grav.Z * 1.25);
                }
                if (Zchange > .025)
                {
                    grav.Z = (float)(grav.Z * 1.125);
                }
                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                float postemp     = (pos.Z - terraintemp);
                if (postemp > 2.5f)
                {
                    grav.Z = (float)(grav.Z * 1.037125);
                }
                //End Experimental Values
            }
            if ((m_flags & (VehicleFlag.NO_X)) != 0)
            {
                m_dir.X = 0;
            }
            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
            {
                m_dir.Y = 0;
            }
            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
            {
                m_dir.Z = 0;
            }

            m_lastPositionVector = d.BodyGetPosition(Body);

            // Apply velocity
            d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
            // apply gravity force
            d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);

            // apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);

            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
        } // end MoveLinear()
        internal void Step()
        {
            IntPtr Body = rootPrim.Body;

            d.Mass dmass;
            d.BodyGetMass(Body, out dmass);

            d.Quaternion rot     = d.BodyGetQuaternion(Body);
            Quaternion   objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
            Quaternion   rotq    = objrotq;                                    // rotq = rotation of object

            rotq *= m_referenceFrame;                                          // rotq is now rotation in vehicle reference frame
            Quaternion irotq = Quaternion.Inverse(rotq);

            d.Vector3 dvtmp;
            Vector3   tmpV;
            Vector3   curVel;                // velocity in world
            Vector3   curAngVel;             // angular velocity in world
            Vector3   force  = Vector3.Zero; // actually linear aceleration until mult by mass in world frame
            Vector3   torque = Vector3.Zero; // actually angular aceleration until mult by Inertia in vehicle frame

            d.Vector3 dtorque = new d.Vector3();

            dvtmp    = d.BodyGetLinearVel(Body);
            curVel.X = dvtmp.X;
            curVel.Y = dvtmp.Y;
            curVel.Z = dvtmp.Z;
            Vector3 curLocalVel = curVel * irotq; // current velocity in  local

            dvtmp       = d.BodyGetAngularVel(Body);
            curAngVel.X = dvtmp.X;
            curAngVel.Y = dvtmp.Y;
            curAngVel.Z = dvtmp.Z;
            Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in  local

            float ldampZ = 0;

            // linear motor
            if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000)
            {
                tmpV  = m_linearMotorDirection - curLocalVel; // velocity error
                tmpV *= m_lmEfect / m_linearMotorTimescale;   // error to correct in this timestep
                tmpV *= rotq;                                 // to world

                if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
                {
                    tmpV.Z = 0;
                }

                if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0)
                {
                    // have offset, do it now
                    tmpV *= dmass.mass;
                    d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z);
                }
                else
                {
                    force.X += tmpV.X;
                    force.Y += tmpV.Y;
                    force.Z += tmpV.Z;
                }

                m_lmEfect *= m_lmDecay;
//                m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared();
                m_ffactor = 0.0f;
            }
            else
            {
                m_lmEfect = 0;
                m_ffactor = 1f;
            }

            // hover
            if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero)
            {
                //                d.Vector3 pos = d.BodyGetPosition(Body);
                d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom);
                pos.Z -= 0.21f; // minor offset that seems to be always there in sl

                float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
                float perr;

                // default to global but don't go underground
                perr = m_VhoverHeight - pos.Z;

                if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0)
                {
                    if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                    {
                        perr += _pParentScene.GetWaterLevel();
                    }
                    else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                    {
                        perr += t;
                    }
                    else
                    {
                        float w = _pParentScene.GetWaterLevel();
                        if (t > w)
                        {
                            perr += t;
                        }
                        else
                        {
                            perr += w;
                        }
                    }
                }
                else if (t > m_VhoverHeight)
                {
                    perr = t - pos.Z;
                }
                ;

                if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1)
                {
                    ldampZ = m_VhoverEfficiency * m_invtimestep;

                    perr *= (1.0f + ldampZ) / m_VhoverTimescale;

                    //                    force.Z += perr - curVel.Z * tmp;
                    force.Z += perr;
                    ldampZ  *= -curVel.Z;

                    force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
                }
                else // no buoyancy
                {
                    force.Z += _pParentScene.gravityz;
                }
            }
            else
            {
                // default gravity and Buoyancy
                force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy);
            }

            // linear deflection
            if (m_linearDeflectionEfficiency > 0)
            {
                float len = curVel.Length();
                if (len > 0.01) // if moving
                {
                    Vector3 atAxis;
                    atAxis  = Xrot(rotq); // where are we pointing to
                    atAxis *= len;        // make it same size as world velocity vector

                    tmpV    = -atAxis;    // oposite direction
                    atAxis -= curVel;     // error to one direction
                    len     = atAxis.LengthSquared();

                    tmpV -= curVel; // error to oposite
                    float lens = tmpV.LengthSquared();

                    if (len > 0.01 || lens > 0.01) // do nothing if close enougth
                    {
                        if (len < lens)
                        {
                            tmpV = atAxis;
                        }

                        tmpV    *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep
                        force.X += tmpV.X;
                        force.Y += tmpV.Y;
                        if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0)
                        {
                            force.Z += tmpV.Z;
                        }
                    }
                }
            }

            // linear friction/damping
            if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0)
            {
                tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X;
                tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y;
                tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z;
                tmpV  *= rotq; // to world

                if (ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z))
                {
                    tmpV.Z = ldampZ;
                }
                force.X += tmpV.X;
                force.Y += tmpV.Y;
                force.Z += tmpV.Z;
            }

            // vertical atractor
            if (m_verticalAttractionTimescale < 300)
            {
                float roll;
                float pitch;



                float ftmp = m_invtimestep / m_verticalAttractionTimescale / m_verticalAttractionTimescale;

                float ftmp2;
                ftmp2     = 0.5f * m_verticalAttractionEfficiency * m_invtimestep;
                m_amdampX = ftmp2;

                m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency;

                GetRollPitch(irotq, out roll, out pitch);

                if (roll > halfpi)
                {
                    roll = pi - roll;
                }
                else if (roll < -halfpi)
                {
                    roll = -pi - roll;
                }

                float effroll = pitch / halfpi;
                effroll *= effroll;
                effroll  = 1 - effroll;
                effroll *= roll;


                torque.X += effroll * ftmp;

                if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)
                {
                    float effpitch = roll / halfpi;
                    effpitch *= effpitch;
                    effpitch  = 1 - effpitch;
                    effpitch *= pitch;

                    torque.Y += effpitch * ftmp;
                }

                if (m_bankingEfficiency != 0 && Math.Abs(effroll) > 0.01)
                {
                    float broll = effroll;

                    /*
                     *                  if (broll > halfpi)
                     *                      broll = pi - broll;
                     *                  else if (broll < -halfpi)
                     *                      broll = -pi - broll;
                     */
                    broll *= m_bankingEfficiency;
                    if (m_bankingMix != 0)
                    {
                        float vfact = Math.Abs(curLocalVel.X) / 10.0f;
                        if (vfact > 1.0f)
                        {
                            vfact = 1.0f;
                        }

                        if (curLocalVel.X >= 0)
                        {
                            broll *= (1 + (vfact - 1) * m_bankingMix);
                        }
                        else
                        {
                            broll *= -(1 + (vfact - 1) * m_bankingMix);
                        }
                    }
                    // make z rot be in world Z not local as seems to be in sl

                    broll = broll / m_bankingTimescale;


                    tmpV  = Zrot(irotq);
                    tmpV *= broll;

                    torque.X += tmpV.X;
                    torque.Y += tmpV.Y;
                    torque.Z += tmpV.Z;

                    m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale;
                    m_amdampY = m_amdampZ;
                }
                else
                {
                    m_amdampZ = 1 / m_angularFrictionTimescale.Z;
                    m_amdampY = m_amdampX;
                }
            }
            else
            {
                m_ampwr   = 1.0f;
                m_amdampX = 1 / m_angularFrictionTimescale.X;
                m_amdampY = 1 / m_angularFrictionTimescale.Y;
                m_amdampZ = 1 / m_angularFrictionTimescale.Z;
            }

            // angular motor
            if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000)
            {
                tmpV      = m_angularMotorDirection - curLocalAngVel; // velocity error
                tmpV     *= m_amEfect / m_angularMotorTimescale;      // error to correct in this timestep
                torque.X += tmpV.X * m_ampwr;
                torque.Y += tmpV.Y * m_ampwr;
                torque.Z += tmpV.Z;

                m_amEfect *= m_amDecay;
            }
            else
            {
                m_amEfect = 0;
            }

            // angular deflection
            if (m_angularDeflectionEfficiency > 0)
            {
                Vector3 dirv;

                if (curLocalVel.X > 0.01f)
                {
                    dirv = curLocalVel;
                }
                else if (curLocalVel.X < -0.01f)
                {
                    // use oposite
                    dirv = -curLocalVel;
                }
                else
                {
                    // make it fall into small positive x case
                    dirv.X = 0.01f;
                    dirv.Y = curLocalVel.Y;
                    dirv.Z = curLocalVel.Z;
                }

                float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale;

                if (Math.Abs(dirv.Z) > 0.01)
                {
                    torque.Y += -(float)Math.Atan2(dirv.Z, dirv.X) * ftmp;
                }

                if (Math.Abs(dirv.Y) > 0.01)
                {
                    torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp;
                }
            }

            // angular friction
            if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0)
            {
                torque.X -= curLocalAngVel.X * m_amdampX;
                torque.Y -= curLocalAngVel.Y * m_amdampY;
                torque.Z -= curLocalAngVel.Z * m_amdampZ;
            }


            if (force.X != 0 || force.Y != 0 || force.Z != 0)
            {
                force *= dmass.mass;
                d.BodyAddForce(Body, force.X, force.Y, force.Z);
            }

            if (torque.X != 0 || torque.Y != 0 || torque.Z != 0)
            {
                torque   *= m_referenceFrame; // to object frame
                dtorque.X = torque.X;
                dtorque.Y = torque.Y;
                dtorque.Z = torque.Z;

                d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque);
                d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame
            }
        }
Exemple #31
0
        public void changeadd()
        {
            // all prims are now created non physical
            IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
            m_targetSpace = targetspace;

            //Console.WriteLine("changeadd 1");
            CreateGeom(m_targetSpace);
            CalcPrimBodyData();

            if (prim_geom != IntPtr.Zero)
            {
                d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
                d.Quaternion myrot = new d.Quaternion();
                Quaternion fake = _orientation;
                myrot.X = fake.X;
                myrot.Y = fake.Y;
                myrot.Z = fake.Z;
                myrot.W = fake.W;
                d.GeomSetQuaternion(prim_geom, ref myrot);
                //                    _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
            }

            changeSelectedStatus(m_isSelected);
        }