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();
        }
        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;

            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();
        }
        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);
        }
 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;
     }
 }
        private void MakeBody()
        {
            //            d.Vector3 dvtmp;
            //            d.Vector3 dbtmp;

            if (m_blockPhysicalReconstruction) // building is blocked
                return;

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

            if (prim_geom == IntPtr.Zero)
            {
                MainConsole.Instance.Warn("[PHYSICS]: Unable to link the linkset.  Root has no geom yet");
                return;
            }

            if (!m_isphysical) // only physical things get a body
                return;

            if (Body != IntPtr.Zero) // who shouldn't have one already ?
            {
                d.BodyDestroy(Body);
                Body = IntPtr.Zero;
                MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body");
            }

            d.Mass objdmass = new d.Mass {};
            d.Matrix3 mymat = new d.Matrix3();
            d.Quaternion myrot = new d.Quaternion();

            Body = d.BodyCreate(_parent_scene.world);
            d.BodySetData(Body, (IntPtr)ActorTypes.Prim);

            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)
            {
                d.Matrix3 mat = new d.Matrix3();
                d.Quaternion quat = new d.Quaternion();
                d.Mass tmpdmass = new d.Mass {};
                Vector3 rcm;

                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)
                    {
                        if (prm.prim_geom == IntPtr.Zero)
                        {
                            MainConsole.Instance.Warn(
                                "[PHYSICS]: Unable to link one of the linkset elements, skipping it.  No geom yet");
                            continue;
                        }

                        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

                        d.GeomClearOffset(prm.prim_geom);
                        d.GeomSetBody(prm.prim_geom, Body);
                        prm.Body = 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.BodySetDamping(Body, .001f, .0002f);
            m_disabled = false;

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

            if (m_targetSpace != _parent_scene.space)
            {
                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)
                        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);

                    if (prm.m_targetSpace != _parent_scene.space)
                    {
                        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);
                    }

                    prm.m_disabled = false;
                    _parent_scene.addActivePrim(prm);
                }
            }
            // 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);
        }
 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;
 }
 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
 }