From 4f05347246c2aedc8adb43772e6dc9fb92ef6c9d Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Tue, 4 Dec 2007 02:51:09 +0000 Subject: [PATCH] * Split out the ODEPlugin Nested classes. --- .../Region/Physics/OdePlugin/ODECharacter.cs | 477 +++++++ OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 686 ++++++++++ OpenSim/Region/Physics/OdePlugin/OdePlugin.cs | 1142 +---------------- 3 files changed, 1166 insertions(+), 1139 deletions(-) create mode 100644 OpenSim/Region/Physics/OdePlugin/ODECharacter.cs create mode 100644 OpenSim/Region/Physics/OdePlugin/ODEPrim.cs diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs new file mode 100644 index 0000000000..cda6af21f7 --- /dev/null +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -0,0 +1,477 @@ +using System; +using System.Collections.Generic; +using Axiom.Math; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdeCharacter : PhysicsActor + { + private PhysicsVector _position; + private d.Vector3 _zeroPosition; + private bool _zeroFlag = false; + private bool m_lastUpdateSent = false; + private PhysicsVector _velocity; + private PhysicsVector _target_velocity; + private PhysicsVector _acceleration; + private PhysicsVector m_rotationalVelocity; + private static float PID_D = 3020.0f; + private static float PID_P = 7000.0f; + private static float POSTURE_SERVO = 10000.0f; + public static float CAPSULE_RADIUS = 0.5f; + public float CAPSULE_LENGTH = 0.79f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_wascolliding = false; + private bool m_wascollidingGround = false; + private bool m_alwaysRun = false; + private bool m_hackSentFall = false; + private bool m_hackSentFly = false; + private string m_name = ""; + + private bool[] m_colliderarr = new bool[11]; + private bool[] m_colliderGroundarr = new bool[11]; + + + private bool jumping = false; + //private float gravityAccel; + public IntPtr Body; + private OdeScene _parent_scene; + public IntPtr Shell; + public d.Mass ShellMass; + public bool collidelock = false; + + public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos) + { + _velocity = new PhysicsVector(); + _target_velocity = new PhysicsVector(); + _position = pos; + _acceleration = new PhysicsVector(); + _parent_scene = parent_scene; + + for (int i = 0; i < 11; i++) + { + m_colliderarr[i] = false; + } + + lock (OdeScene.OdeLock) + { + + Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); + d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f); + Body = d.BodyCreate(parent_scene.world); + d.BodySetMass(Body, ref ShellMass); + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + d.GeomSetBody(Shell, Body); + } + m_name = avName; + parent_scene.geom_name_map[Shell] = avName; + parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + public override bool IsPhysical + { + get { return false; } + set { return; } + } + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + public override bool Flying + { + get { return flying; } + set { flying = value; } + } + public override bool IsColliding + { + + get { return m_iscolliding; } + set + { + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderarr[i] = m_colliderarr[i + 1]; + } + } + m_colliderarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2 * truecount) + { + m_iscolliding = false; + } + else + { + m_iscolliding = true; + } + if (m_wascolliding != m_iscolliding) + { + base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascolliding = m_iscolliding; + } + } + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderGroundarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; + } + } + m_colliderGroundarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderGroundarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2 * truecount) + { + m_iscollidingGround = false; + } + else + { + m_iscollidingGround = true; + } + if (m_wascollidingGround != m_iscollidingGround) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascollidingGround = m_iscollidingGround; + } + } + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override PhysicsVector Position + { + get { return _position; } + set + { + lock (OdeScene.OdeLock) + { + d.BodySetPosition(Body, value.X, value.Y, value.Z); + _position = value; + } + } + } + public override PhysicsVector RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + public override PhysicsVector Size + { + get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } + set + { + lock (OdeScene.OdeLock) + { + PhysicsVector SetSize = value; + float prevCapsule = CAPSULE_LENGTH; + float capsuleradius = CAPSULE_RADIUS; + capsuleradius = 0.2f; + + CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z * 0.43f))); // subtract 43% of the size + d.BodyDestroy(Body); + d.GeomDestroy(Shell); + //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH)); + Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH); + d.MassSetCapsule(out ShellMass, 50.0f, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + Body = d.BodyCreate(_parent_scene.world); + d.BodySetMass(Body, ref ShellMass); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z + Math.Abs(CAPSULE_LENGTH - prevCapsule)); + d.GeomSetBody(Shell, Body); + } + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + } + + public override PrimitiveBaseShape Shape + { + set + { + return; + } + } + + public override PhysicsVector Velocity + { + get { return _velocity; } + set { _target_velocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set { } + } + + public override PhysicsVector Acceleration + { + get { return _acceleration; } + } + + public void SetAcceleration(PhysicsVector accel) + { + _acceleration = accel; + } + + public override void AddForce(PhysicsVector force) + { + + _target_velocity.X += force.X; + _target_velocity.Y += force.Y; + _target_velocity.Z += force.Z; + + //m_lastUpdateSent = false; + } + public void doForce(PhysicsVector force) + { + if (!collidelock) + { + d.BodyAddForce(Body, force.X, force.Y, force.Z); + + // ok -- let's stand up straight! + d.Vector3 feet; + d.Vector3 head; + d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet); + d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head); + float posture = head.Z - feet.Z; + + // restoring force proportional to lack of posture: + float servo = (2.5f - posture) * POSTURE_SERVO; + d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); + d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); + //m_lastUpdateSent = false; + + } + + } + public override void SetMomentum(PhysicsVector momentum) + { + + } + + public void Move(float timeStep) + { + // no lock; for now it's only called from within Simulate() + PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + float movementdivisor = 1f; + + if (!m_alwaysRun) + { + movementdivisor = 1.3f; + } + else + { + movementdivisor = 0.8f; + + } + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = d.BodyGetPosition(Body); + } + d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D + (_zeroPosition.X - pos.X) * PID_P; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D + (_zeroPosition.Y - pos.Y) * PID_P; + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + } + } + else + { + + _zeroFlag = false; + if (m_iscolliding || flying) + { + + vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * PID_D; + vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * PID_D; + } + if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) + { + d.Vector3 pos = d.BodyGetPosition(Body); + vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + else if (!m_iscolliding && !flying) + { + d.Vector3 pos = d.BodyGetPosition(Body); + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + + } + + + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * PID_D; + } + } + if (flying) + { + vec.Z += 10.0f; + } + + + doForce(vec); + } + + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + d.Vector3 vec = d.BodyGetPosition(Body); + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + if (vec.X < 0.0f) vec.X = 0.0f; + if (vec.Y < 0.0f) vec.Y = 0.0f; + if (vec.X > 255.95f) vec.X = 255.95f; + if (vec.Y > 255.95f) vec.Y = 255.95f; + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); + string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + if (primScenAvatarIn == "0") + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in space with no prim. Arr:':" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + else + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in Prim space':" + primScenAvatarIn + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + + } + } + else + { + m_lastUpdateSent = false; + vec = d.BodyGetLinearVel(Body); + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + + _velocity.Z = (vec.Z); + if (_velocity.Z < -6 && !m_hackSentFall) + { + m_hackSentFall = true; + base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else if (flying && !m_hackSentFly) + { + //m_hackSentFly = true; + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else + { + m_hackSentFly = false; + m_hackSentFall = false; + } + } + } + + public void Destroy() + { + lock (OdeScene.OdeLock) + { + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + d.BodyDestroy(Body); + } + } + } + +} diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs new file mode 100644 index 0000000000..f423e827f6 --- /dev/null +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -0,0 +1,686 @@ +using System; +using System.Collections.Generic; +using Axiom.Math; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + + public class OdePrim : PhysicsActor + { + public PhysicsVector _position; + private PhysicsVector _velocity; + private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f); + private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f); + private PhysicsVector m_rotationalVelocity; + private PhysicsVector _size; + private PhysicsVector _acceleration; + private Quaternion _orientation; + private PhysicsVector m_taintposition; + private PhysicsVector m_taintsize; + private Quaternion m_taintrot; + private bool m_taintshape = false; + private bool m_taintPhysics = false; + public bool m_taintremove = false; + + private IMesh _mesh; + private PrimitiveBaseShape _pbs; + private OdeScene _parent_scene; + public IntPtr m_targetSpace = (IntPtr)0; + public IntPtr prim_geom; + public IntPtr _triMeshData; + private bool iscolliding = false; + private bool m_isphysical = false; + private bool m_throttleUpdates = false; + private int throttleCounter = 0; + public bool outofBounds = false; + + public bool _zeroFlag = false; + private bool m_lastUpdateSent = false; + + public IntPtr Body = (IntPtr)0; + private String m_primName; + private PhysicsVector _target_velocity; + public d.Mass pMass; + private const float MassMultiplier = 150f; // Ref: Water: 1000kg.. this iset to 500 + private int debugcounter = 0; + + + public OdePrim(String primName, OdeScene parent_scene, IntPtr targetSpace, PhysicsVector pos, PhysicsVector size, + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical) + { + + + _velocity = new PhysicsVector(); + _position = pos; + m_taintposition = pos; + if (_position.X > 257) + { + _position.X = 257; + } + if (_position.X < 0) + { + _position.X = 0; + } + if (_position.Y > 257) + { + _position.Y = 257; + } + if (_position.Y < 0) + { + _position.Y = 0; + } + + _size = size; + m_taintsize = _size; + _acceleration = new PhysicsVector(); + m_rotationalVelocity = PhysicsVector.Zero; + _orientation = rotation; + m_taintrot = _orientation; + _mesh = mesh; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = targetSpace; + + if (pos.Z < 0) + m_isphysical = false; + else + { + m_isphysical = pisPhysical; + // If we're physical, we need to be in the master space for now. + // linksets *should* be in a space together.. but are not currently + if (m_isphysical) + m_targetSpace = _parent_scene.space; + + } + m_primName = primName; + + + + lock (OdeScene.OdeLock) + { + if (mesh != null) + { + setMesh(parent_scene, mesh); + } + else + { + prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.W = rotation.w; + myrot.X = rotation.x; + myrot.Y = rotation.y; + myrot.Z = rotation.z; + d.GeomSetQuaternion(prim_geom, ref myrot); + + + if (m_isphysical && Body == (IntPtr)0) + { + enableBody(); + } + parent_scene.geom_name_map[prim_geom] = primName; + parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + // don't do .add() here; old geoms get recycled with the same hash + } + } + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + public void enableBody() + { + // 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.W = _orientation.w; + myrot.X = _orientation.x; + myrot.Y = _orientation.y; + myrot.Z = _orientation.z; + d.BodySetQuaternion(Body, ref myrot); + d.GeomSetBody(prim_geom, Body); + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, 20); + + _parent_scene.addActivePrim(this); + } + public void setMass() + { + //Sets Mass based on member MassMultiplier. + if (Body != (IntPtr)0) + { + d.MassSetBox(out pMass, (_size.X * _size.Y * _size.Z * MassMultiplier), _size.X, _size.Y, _size.Z); + d.BodySetMass(Body, ref pMass); + } + } + public void disableBody() + { + //this kills the body so things like 'mesh' can re-create it. + if (Body != (IntPtr)0) + { + _parent_scene.remActivePrim(this); + d.BodyDestroy(Body); + Body = (IntPtr)0; + } + } + public void setMesh(OdeScene parent_scene, IMesh mesh) + { + //Kill Body so that mesh can re-make the geom + if (IsPhysical && Body != (IntPtr)0) + { + disableBody(); + } + float[] vertexList = mesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory + int[] indexList = mesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage + int VertexCount = vertexList.GetLength(0) / 3; + int IndexCount = indexList.GetLength(0); + + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertexList, 3 * sizeof(float), VertexCount, indexList, IndexCount, + 3 * sizeof(int)); + d.GeomTriMeshDataPreprocess(_triMeshData); + + prim_geom = d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null); + + if (IsPhysical && Body == (IntPtr)0) + { + // Recreate the body + enableBody(); + } + } + public void ProcessTaints(float timestep) + { + if (m_taintposition != _position) + Move(timestep); + + if (m_taintrot != _orientation) + rotate(timestep); + // + + if (m_taintPhysics != m_isphysical) + changePhysicsStatus(timestep); + // + + if (m_taintsize != _size) + changesize(timestep); + // + + if (m_taintshape) + changeshape(timestep); + // + + } + public void Move(float timestep) + { + if (m_isphysical) + { + // This is a fallback.. May no longer be necessary. + if (Body == (IntPtr)0) + enableBody(); + //Prim auto disable after 20 frames, + ///if you move it, re-enable the prim manually. + d.BodyEnable(Body); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + } + else + { + string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + if (primScenAvatarIn == "0") + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Prim " + m_primName + " in space with no prim: " + primScenAvatarIn + ". Expected to be at: " + m_targetSpace.ToString() + " . Arr:': " + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + else + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Prim " + m_primName + " in Prim space with prim: " + primScenAvatarIn + ". Expected to be at: " + m_targetSpace.ToString() + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + m_targetSpace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.SpaceAdd(m_targetSpace, prim_geom); + } + + m_taintposition = _position; + } + public void rotate(float timestep) + { + + d.Quaternion myrot = new d.Quaternion(); + myrot.W = _orientation.w; + myrot.X = _orientation.x; + myrot.Y = _orientation.y; + myrot.Z = _orientation.z; + d.GeomSetQuaternion(prim_geom, ref myrot); + if (m_isphysical && Body != (IntPtr)0) + { + d.BodySetQuaternion(Body, ref myrot); + } + + m_taintrot = _orientation; + } + public void changePhysicsStatus(float timestap) + { + if (m_isphysical == true) + { + if (Body == (IntPtr)0) + { + enableBody(); + } + + } + else + { + if (Body != (IntPtr)0) + { + disableBody(); + } + } + + + m_taintPhysics = m_isphysical; + } + public void changesize(float timestamp) + { + string oldname = _parent_scene.geom_name_map[prim_geom]; + + // Cleanup of old prim geometry + if (_mesh != null) + { + // Cleanup meshing here + } + //kill body to rebuild + if (IsPhysical && Body != (IntPtr)0) + { + disableBody(); + } + if (d.SpaceQuery(m_targetSpace, prim_geom)) + { + d.SpaceRemove(m_targetSpace, prim_geom); + } + d.GeomDestroy(prim_geom); + + // we don't need to do space calculation because the client sends a position update also. + + // Construction of new prim + if (this._parent_scene.needsMeshing(_pbs)) + { + + + // Don't need to re-enable body.. it's done in SetMesh + IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size); + // createmesh returns null when it's a shape that isn't a cube. + if (mesh != null) + { + setMesh(_parent_scene, mesh); + } + else + { + prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.W = _orientation.w; + myrot.X = _orientation.x; + myrot.Y = _orientation.y; + myrot.Z = _orientation.z; + d.GeomSetQuaternion(prim_geom, ref myrot); + + + } + } + else + { + prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + 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)0) + { + // Re creates body on size. + // EnableBody also does setMass() + enableBody(); + d.BodyEnable(Body); + } + + } + + + _parent_scene.geom_name_map[prim_geom] = oldname; + + + m_taintsize = _size; + } + 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)0) + { + disableBody(); + } + d.GeomDestroy(prim_geom); + if (_mesh != null) + { + + d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + + } + + // Construction of new prim + if (this._parent_scene.needsMeshing(_pbs)) + { + IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size); + if (mesh != null) + { + setMesh(_parent_scene, mesh); + } + else + { + prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + } + else + { + prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + if (IsPhysical && Body == (IntPtr)0) + { + //re-create new body + enableBody(); + } + else + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.W = _orientation.w; + myrot.X = _orientation.x; + myrot.Y = _orientation.y; + myrot.Z = _orientation.z; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + _parent_scene.geom_name_map[prim_geom] = oldname; + + + + + m_taintshape = false; + } + public override bool IsPhysical + { + get { return m_isphysical; } + set { m_isphysical = value; } + } + public void setPrimForRemoval() + { + m_taintremove = true; + } + + public override bool Flying + { + get + { + return false; //no flying prims for you + } + set { } + } + + public override bool IsColliding + { + get { return iscolliding; } + set { iscolliding = value; } + } + public override bool CollidingGround + { + get { return false; } + set { return; } + } + public override bool CollidingObj + { + get { return false; } + set { return; } + } + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override PhysicsVector Position + { + get { return _position; } + + + set + { + _position = value; + + } + } + + public override PhysicsVector Size + { + get { return _size; } + set + { + _size = value; + + } + } + + public override PrimitiveBaseShape Shape + { + set + { + _pbs = value; + + } + } + + public override PhysicsVector Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + PhysicsVector returnVelocity = new PhysicsVector(); + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; + return returnVelocity; + } + set { _velocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return _orientation; } + set + { + _orientation = value; + + } + } + + public override PhysicsVector Acceleration + { + get { return _acceleration; } + } + + + public void SetAcceleration(PhysicsVector accel) + { + _acceleration = accel; + } + + public override void AddForce(PhysicsVector force) + { + } + + public override PhysicsVector RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + + if (Body != (IntPtr)0) + { + d.Vector3 vec = d.BodyGetPosition(Body); + d.Quaternion ori = d.BodyGetQuaternion(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + PhysicsVector l_position = new PhysicsVector(); + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + if (vec.X < 0.0f) vec.X = 0.0f; + if (vec.Y < 0.0f) vec.Y = 0.0f; + if (vec.X > 255.95f) vec.X = 255.95f; + if (vec.Y > 255.95f) vec.Y = 255.95f; + m_lastposition = _position; + + l_position.X = vec.X; + l_position.Y = vec.Y; + l_position.Z = vec.Z; + if (l_position.Z < 0) + { + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. + + + + + //IsPhysical = false; + base.RaiseOutOfBounds(_position); + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + base.RequestPhysicsterseUpdate(); + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; + } + + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)) + { + + _zeroFlag = true; + } + else + { + //System.Console.WriteLine(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + } + + + + if (_zeroFlag) + { + // Supposedly this is supposed to tell SceneObjectGroup that + // no more updates need to be sent.. + // but it seems broken. + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + //_orientation.w = 0f; + //_orientation.x = 0f; + //_orientation.y = 0f; + //_orientation.z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = true; + } + + } + else + { + m_lastVelocity = _velocity; + + _position = l_position; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString()); + _orientation.w = ori.W; + _orientation.x = ori.X; + _orientation.y = ori.Y; + _orientation.z = ori.Z; + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > 15) + { + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + m_lastposition = l_position; + } + else + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + } + + } + public override void SetMomentum(PhysicsVector momentum) + { + } + } +} diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs index e8ee33c519..1e05274c1b 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs @@ -1087,1143 +1087,7 @@ namespace OpenSim.Region.Physics.OdePlugin { } } - # region ODE Actors + - public class OdeCharacter : PhysicsActor - { - private PhysicsVector _position; - private d.Vector3 _zeroPosition; - private bool _zeroFlag = false; - private bool m_lastUpdateSent = false; - private PhysicsVector _velocity; - private PhysicsVector _target_velocity; - private PhysicsVector _acceleration; - private PhysicsVector m_rotationalVelocity; - private static float PID_D = 3020.0f; - private static float PID_P = 7000.0f; - private static float POSTURE_SERVO = 10000.0f; - public static float CAPSULE_RADIUS = 0.5f; - public float CAPSULE_LENGTH = 0.79f; - private bool flying = false; - private bool m_iscolliding = false; - private bool m_iscollidingGround = false; - private bool m_wascolliding = false; - private bool m_wascollidingGround = false; - private bool m_alwaysRun = false; - private bool m_hackSentFall = false; - private bool m_hackSentFly = false; - private string m_name = ""; - - private bool[] m_colliderarr = new bool[11]; - private bool[] m_colliderGroundarr = new bool[11]; - - - private bool jumping = false; - //private float gravityAccel; - public IntPtr Body; - private OdeScene _parent_scene; - public IntPtr Shell; - public d.Mass ShellMass; - public bool collidelock = false; - - public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos) - { - _velocity = new PhysicsVector(); - _target_velocity = new PhysicsVector(); - _position = pos; - _acceleration = new PhysicsVector(); - _parent_scene = parent_scene; - - for (int i = 0; i < 11; i++) - { - m_colliderarr[i] = false; - } - - lock (OdeScene.OdeLock) - { - - Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); - d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f); - Body = d.BodyCreate(parent_scene.world); - d.BodySetMass(Body, ref ShellMass); - d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); - d.GeomSetBody(Shell, Body); - } - m_name = avName; - parent_scene.geom_name_map[Shell] = avName; - parent_scene.actor_name_map[Shell] = (PhysicsActor)this; - } - public override int PhysicsActorType - { - get { return (int)ActorTypes.Agent; } - set { return; } - } - public override bool SetAlwaysRun - { - get { return m_alwaysRun; } - set { m_alwaysRun = value;} - } - public override bool IsPhysical - { - get { return false; } - set { return; } - } - public override bool ThrottleUpdates - { - get { return false; } - set { return; } - } - public override bool Flying - { - get { return flying; } - set { flying = value; } - } - public override bool IsColliding - { - - get { return m_iscolliding; } - set - { - int i; - int truecount=0; - int falsecount=0; - - if (m_colliderarr.Length >= 10) - { - for (i = 0; i < 10; i++) - { - m_colliderarr[i] = m_colliderarr[i + 1]; - } - } - m_colliderarr[10] = value; - - for (i = 0; i < 11; i++) - { - if (m_colliderarr[i]) - { - truecount++; - } - else - { - falsecount++; - } - } - - // Equal truecounts and false counts means we're colliding with something. - - if (falsecount > 1.2 * truecount) - { - m_iscolliding = false; - } - else - { - m_iscolliding = true; - } - if (m_wascolliding != m_iscolliding) - { - base.SendCollisionUpdate(new CollisionEventUpdate()); - } - m_wascolliding = m_iscolliding; - } - } - public override bool CollidingGround - { - get { return m_iscollidingGround; } - set - { - int i; - int truecount = 0; - int falsecount = 0; - - if (m_colliderGroundarr.Length >= 10) - { - for (i = 0; i < 10; i++) - { - m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; - } - } - m_colliderGroundarr[10] = value; - - for (i = 0; i < 11; i++) - { - if (m_colliderGroundarr[i]) - { - truecount++; - } - else - { - falsecount++; - } - } - - // Equal truecounts and false counts means we're colliding with something. - - if (falsecount > 1.2 * truecount) - { - m_iscollidingGround = false; - } - else - { - m_iscollidingGround = true; - } - if (m_wascollidingGround != m_iscollidingGround) - { - //base.SendCollisionUpdate(new CollisionEventUpdate()); - } - m_wascollidingGround = m_iscollidingGround; - } - } - public override bool CollidingObj - { - get { return false; } - set { return; } - } - - public override PhysicsVector Position - { - get { return _position; } - set - { - lock (OdeScene.OdeLock) - { - d.BodySetPosition(Body, value.X, value.Y, value.Z); - _position = value; - } - } - } - public override PhysicsVector RotationalVelocity - { - get { return m_rotationalVelocity; } - set { m_rotationalVelocity = value; } - } - public override PhysicsVector Size - { - get { return new PhysicsVector(CAPSULE_RADIUS*2, CAPSULE_RADIUS*2, CAPSULE_LENGTH); } - set { - lock (OdeScene.OdeLock) - { - PhysicsVector SetSize = value; - float prevCapsule = CAPSULE_LENGTH; - float capsuleradius = CAPSULE_RADIUS; - capsuleradius = 0.2f; - - CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z * 0.43f))); // subtract 43% of the size - d.BodyDestroy(Body); - d.GeomDestroy(Shell); - //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH)); - Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH); - d.MassSetCapsule(out ShellMass, 50.0f, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); - Body = d.BodyCreate(_parent_scene.world); - d.BodySetMass(Body, ref ShellMass); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z + Math.Abs(CAPSULE_LENGTH-prevCapsule)); - d.GeomSetBody(Shell, Body); - } - _parent_scene.geom_name_map[Shell] = m_name; - _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; - } - } - - public override PrimitiveBaseShape Shape - { - set - { - return; - } - } - - public override PhysicsVector Velocity - { - get { return _velocity; } - set { _target_velocity = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get { return Quaternion.Identity; } - set { } - } - - public override PhysicsVector Acceleration - { - get { return _acceleration; } - } - - public void SetAcceleration(PhysicsVector accel) - { - _acceleration = accel; - } - - public override void AddForce(PhysicsVector force) - { - - _target_velocity.X += force.X; - _target_velocity.Y += force.Y; - _target_velocity.Z += force.Z; - - //m_lastUpdateSent = false; - } - public void doForce(PhysicsVector force) - { - if (!collidelock) - { - d.BodyAddForce(Body, force.X, force.Y, force.Z); - - // ok -- let's stand up straight! - d.Vector3 feet; - d.Vector3 head; - d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet); - d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head); - float posture = head.Z - feet.Z; - - // restoring force proportional to lack of posture: - float servo = (2.5f - posture) * POSTURE_SERVO; - d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); - d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); - //m_lastUpdateSent = false; - - } - - } - public override void SetMomentum(PhysicsVector momentum) - { - - } - - public void Move(float timeStep) - { - // no lock; for now it's only called from within Simulate() - PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); - float movementdivisor = 1f; - - if (!m_alwaysRun) - { - movementdivisor = 1.3f; - } - else - { - movementdivisor = 0.8f; - - } - - // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding) - { - // keep track of where we stopped. No more slippin' & slidin' - if (!_zeroFlag) - { - _zeroFlag = true; - _zeroPosition = d.BodyGetPosition(Body); - } - d.Vector3 pos = d.BodyGetPosition(Body); - vec.X = (_target_velocity.X - vel.X)*PID_D + (_zeroPosition.X - pos.X)*PID_P; - vec.Y = (_target_velocity.Y - vel.Y)*PID_D + (_zeroPosition.Y - pos.Y)*PID_P; - if (flying) - { - vec.Z = (_target_velocity.Z - vel.Z)*PID_D + (_zeroPosition.Z - pos.Z)*PID_P; - } - } - else - { - - _zeroFlag = false; - if (m_iscolliding || flying) - { - - vec.X = ((_target_velocity.X/movementdivisor) - vel.X) * PID_D; - vec.Y = ((_target_velocity.Y/movementdivisor) - vel.Y) * PID_D; - } - if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) - { - d.Vector3 pos = d.BodyGetPosition(Body); - vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - if (_target_velocity.X > 0) - { - vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; - } - if (_target_velocity.Y > 0) - { - vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; - } - } - else if (!m_iscolliding && !flying) - { - d.Vector3 pos = d.BodyGetPosition(Body); - if (_target_velocity.X > 0) - { - vec.X = ((_target_velocity.X - vel.X)/1.2f) * PID_D; - } - if (_target_velocity.Y > 0) - { - vec.Y = ((_target_velocity.Y - vel.Y)/1.2f) * PID_D; - } - - } - - - if (flying) - { - vec.Z = (_target_velocity.Z - vel.Z)*PID_D; - } - } - if (flying) - { - vec.Z += 10.0f; - } - - - doForce(vec); - } - - public void UpdatePositionAndVelocity() - { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - d.Vector3 vec = d.BodyGetPosition(Body); - - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - if (vec.X < 0.0f) vec.X = 0.0f; - if (vec.Y < 0.0f) vec.Y = 0.0f; - if (vec.X > 255.95f) vec.X = 255.95f; - if (vec.Y > 255.95f) vec.Y = 255.95f; - - _position.X = vec.X; - _position.Y = vec.Y; - _position.Z = vec.Z; - - if (_zeroFlag) - { - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - if (!m_lastUpdateSent) - { - m_lastUpdateSent = true; - base.RequestPhysicsterseUpdate(); - string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - if (primScenAvatarIn == "0") - { - OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in space with no prim. Arr:':" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); - } - else - { - OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in Prim space':" + primScenAvatarIn + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); - } - - } - } - else - { - m_lastUpdateSent = false; - vec = d.BodyGetLinearVel(Body); - _velocity.X = (vec.X); - _velocity.Y = (vec.Y); - - _velocity.Z = (vec.Z); - if (_velocity.Z < -6 && !m_hackSentFall) - { - m_hackSentFall = true; - base.SendCollisionUpdate(new CollisionEventUpdate()); - } - else if (flying && !m_hackSentFly) - { - //m_hackSentFly = true; - //base.SendCollisionUpdate(new CollisionEventUpdate()); - } - else - { - m_hackSentFly = false; - m_hackSentFall = false; - } - } - } - - public void Destroy() - { - lock (OdeScene.OdeLock) - { - d.GeomDestroy(Shell); - _parent_scene.geom_name_map.Remove(Shell); - d.BodyDestroy(Body); - } - } - } - - public class OdePrim : PhysicsActor - { - public PhysicsVector _position; - private PhysicsVector _velocity; - private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f,0.0f,0.0f); - private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f); - private PhysicsVector m_rotationalVelocity; - private PhysicsVector _size; - private PhysicsVector _acceleration; - private Quaternion _orientation; - private PhysicsVector m_taintposition; - private PhysicsVector m_taintsize; - private Quaternion m_taintrot; - private bool m_taintshape = false; - private bool m_taintPhysics = false; - public bool m_taintremove = false; - - private IMesh _mesh; - private PrimitiveBaseShape _pbs; - private OdeScene _parent_scene; - public IntPtr m_targetSpace = (IntPtr)0; - public IntPtr prim_geom; - public IntPtr _triMeshData; - private bool iscolliding = false; - private bool m_isphysical = false; - private bool m_throttleUpdates = false; - private int throttleCounter = 0; - public bool outofBounds = false; - - public bool _zeroFlag = false; - private bool m_lastUpdateSent = false; - - public IntPtr Body = (IntPtr) 0; - private String m_primName; - private PhysicsVector _target_velocity; - public d.Mass pMass; - private const float MassMultiplier = 150f; // Ref: Water: 1000kg.. this iset to 500 - private int debugcounter = 0; - - - public OdePrim(String primName, OdeScene parent_scene, IntPtr targetSpace, PhysicsVector pos, PhysicsVector size, - Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical) - { - - - _velocity = new PhysicsVector(); - _position = pos; - m_taintposition = pos; - if (_position.X > 257) - { - _position.X = 257; - } - if (_position.X < 0) - { - _position.X = 0; - } - if (_position.Y > 257) - { - _position.Y = 257; - } - if (_position.Y < 0) - { - _position.Y = 0; - } - - _size = size; - m_taintsize = _size; - _acceleration = new PhysicsVector(); - m_rotationalVelocity = PhysicsVector.Zero; - _orientation = rotation; - m_taintrot = _orientation; - _mesh = mesh; - _pbs = pbs; - - _parent_scene = parent_scene; - m_targetSpace = targetSpace; - - if (pos.Z < 0) - m_isphysical = false; - else - { - m_isphysical = pisPhysical; - // If we're physical, we need to be in the master space for now. - // linksets *should* be in a space together.. but are not currently - if (m_isphysical) - m_targetSpace = _parent_scene.space; - - } - m_primName = primName; - - - - lock (OdeScene.OdeLock) - { - if (mesh != null) - { - setMesh(parent_scene, mesh); - } - else - { - prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - } - - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.W = rotation.w; - myrot.X = rotation.x; - myrot.Y = rotation.y; - myrot.Z = rotation.z; - d.GeomSetQuaternion(prim_geom, ref myrot); - - - if (m_isphysical && Body == (IntPtr)0) { - enableBody(); - } - parent_scene.geom_name_map[prim_geom] = primName; - parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; - // don't do .add() here; old geoms get recycled with the same hash - } - } - public override int PhysicsActorType - { - get { return (int)ActorTypes.Prim; } - set { return; } - } - public override bool SetAlwaysRun - { - get { return false; } - set { return; } - } - public void enableBody() - { - // 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.W = _orientation.w; - myrot.X = _orientation.x; - myrot.Y = _orientation.y; - myrot.Z = _orientation.z; - d.BodySetQuaternion(Body, ref myrot); - d.GeomSetBody(prim_geom, Body); - d.BodySetAutoDisableFlag(Body, true); - d.BodySetAutoDisableSteps(Body,20); - - _parent_scene.addActivePrim(this); - } - public void setMass() - { - //Sets Mass based on member MassMultiplier. - if (Body != (IntPtr)0) - { - d.MassSetBox(out pMass, (_size.X * _size.Y * _size.Z * MassMultiplier), _size.X, _size.Y, _size.Z); - d.BodySetMass(Body, ref pMass); - } - } - public void disableBody() - { - //this kills the body so things like 'mesh' can re-create it. - if (Body != (IntPtr)0) - { - _parent_scene.remActivePrim(this); - d.BodyDestroy(Body); - Body = (IntPtr)0; - } - } - public void setMesh(OdeScene parent_scene, IMesh mesh) - { - //Kill Body so that mesh can re-make the geom - if (IsPhysical && Body != (IntPtr)0) - { - disableBody(); - } - float[] vertexList = mesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory - int[] indexList = mesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage - int VertexCount = vertexList.GetLength(0)/3; - int IndexCount = indexList.GetLength(0); - - _triMeshData = d.GeomTriMeshDataCreate(); - - d.GeomTriMeshDataBuildSimple(_triMeshData, vertexList, 3*sizeof (float), VertexCount, indexList, IndexCount, - 3*sizeof (int)); - d.GeomTriMeshDataPreprocess(_triMeshData); - - prim_geom = d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null); - - if (IsPhysical && Body == (IntPtr)0) - { - // Recreate the body - enableBody(); - } - } - public void ProcessTaints(float timestep) - { - if (m_taintposition != _position) - Move(timestep); - - if (m_taintrot != _orientation) - rotate(timestep); - // - - if (m_taintPhysics != m_isphysical) - changePhysicsStatus(timestep); - // - - if (m_taintsize != _size) - changesize(timestep); - // - - if (m_taintshape) - changeshape(timestep); - // - - } - public void Move(float timestep) - { - if (m_isphysical) - { - // This is a fallback.. May no longer be necessary. - if (Body == (IntPtr)0) - enableBody(); - //Prim auto disable after 20 frames, - ///if you move it, re-enable the prim manually. - d.BodyEnable(Body); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - } - else - { - string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); - int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); - if (primScenAvatarIn == "0") - { - OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Prim " + m_primName + " in space with no prim: " + primScenAvatarIn + ". Expected to be at: " + m_targetSpace.ToString() + " . Arr:': " + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); - } - else - { - OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Prim " + m_primName + " in Prim space with prim: " + primScenAvatarIn + ". Expected to be at: " + m_targetSpace.ToString() + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); - } - m_targetSpace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.SpaceAdd(m_targetSpace, prim_geom); - } - - m_taintposition = _position; - } - public void rotate(float timestep) - { - - d.Quaternion myrot = new d.Quaternion(); - myrot.W = _orientation.w; - myrot.X = _orientation.x; - myrot.Y = _orientation.y; - myrot.Z = _orientation.z; - d.GeomSetQuaternion(prim_geom, ref myrot); - if (m_isphysical && Body != (IntPtr)0) - { - d.BodySetQuaternion(Body, ref myrot); - } - - m_taintrot = _orientation; - } - public void changePhysicsStatus(float timestap) - { - if (m_isphysical == true) - { - if (Body == (IntPtr)0) - { - enableBody(); - } - - } - else - { - if (Body != (IntPtr)0) - { - disableBody(); - } - } - - - m_taintPhysics = m_isphysical; - } - public void changesize(float timestamp) - { - string oldname = _parent_scene.geom_name_map[prim_geom]; - - // Cleanup of old prim geometry - if (_mesh != null) - { - // Cleanup meshing here - } - //kill body to rebuild - if (IsPhysical && Body != (IntPtr)0) - { - disableBody(); - } - if (d.SpaceQuery(m_targetSpace, prim_geom)) - { - d.SpaceRemove(m_targetSpace, prim_geom); - } - d.GeomDestroy(prim_geom); - - // we don't need to do space calculation because the client sends a position update also. - - // Construction of new prim - if (this._parent_scene.needsMeshing(_pbs)) - { - - - // Don't need to re-enable body.. it's done in SetMesh - IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size); - // createmesh returns null when it's a shape that isn't a cube. - if (mesh != null) - { - setMesh(_parent_scene, mesh); - } - else - { - prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.W = _orientation.w; - myrot.X = _orientation.x; - myrot.Y = _orientation.y; - myrot.Z = _orientation.z; - d.GeomSetQuaternion(prim_geom, ref myrot); - - - } - } - else - { - prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - 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)0) - { - // Re creates body on size. - // EnableBody also does setMass() - enableBody(); - d.BodyEnable(Body); - } - - } - - - _parent_scene.geom_name_map[prim_geom] = oldname; - - - m_taintsize = _size; - } - 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)0) - { - disableBody(); - } - d.GeomDestroy(prim_geom); - if (_mesh != null) - { - - d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); - - } - - // Construction of new prim - if (this._parent_scene.needsMeshing(_pbs)) - { - IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size); - if (mesh != null) - { - setMesh(_parent_scene, mesh); - } - else - { - prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - } - } - else - { - prim_geom = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); - } - if (IsPhysical && Body == (IntPtr)0) - { - //re-create new body - enableBody(); - } - else - { - d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); - d.Quaternion myrot = new d.Quaternion(); - myrot.W = _orientation.w; - myrot.X = _orientation.x; - myrot.Y = _orientation.y; - myrot.Z = _orientation.z; - d.GeomSetQuaternion(prim_geom, ref myrot); - } - _parent_scene.geom_name_map[prim_geom] = oldname; - - - - - m_taintshape = false; - } - public override bool IsPhysical - { - get { return m_isphysical; } - set{ m_isphysical = value;} - } - public void setPrimForRemoval() - { - m_taintremove = true; - } - - public override bool Flying - { - get { return false; //no flying prims for you - } - set { } - } - - public override bool IsColliding - { - get { return iscolliding; } - set { iscolliding = value; } - } - public override bool CollidingGround - { - get { return false; } - set { return; } - } - public override bool CollidingObj - { - get { return false; } - set { return; } - } - public override bool ThrottleUpdates - { - get { return m_throttleUpdates; } - set { m_throttleUpdates=value; } - } - - public override PhysicsVector Position - { - get {return _position; } - - - set - { - _position = value; - - } - } - - public override PhysicsVector Size - { - get { return _size; } - set - { - _size = value; - - } - } - - public override PrimitiveBaseShape Shape - { - set - { - _pbs = value; - - } - } - - public override PhysicsVector Velocity - { - get { - // Averate previous velocity with the new one so - // client object interpolation works a 'little' better - PhysicsVector returnVelocity = new PhysicsVector(); - returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; - return returnVelocity; - } - set { _velocity = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override Quaternion Orientation - { - get { return _orientation; } - set - { - _orientation = value; - - } - } - - public override PhysicsVector Acceleration - { - get { return _acceleration; } - } - - - public void SetAcceleration(PhysicsVector accel) - { - _acceleration = accel; - } - - public override void AddForce(PhysicsVector force) - { - } - - public override PhysicsVector RotationalVelocity - { - get{ return m_rotationalVelocity;} - set { m_rotationalVelocity = value; } - } - - public void UpdatePositionAndVelocity() { - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - - if (Body != (IntPtr)0) - { - d.Vector3 vec = d.BodyGetPosition(Body); - d.Quaternion ori = d.BodyGetQuaternion(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - d.Vector3 rotvel = d.BodyGetAngularVel(Body); - - PhysicsVector l_position = new PhysicsVector(); - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - if (vec.X < 0.0f) vec.X = 0.0f; - if (vec.Y < 0.0f) vec.Y = 0.0f; - if (vec.X > 255.95f) vec.X = 255.95f; - if (vec.Y > 255.95f) vec.Y = 255.95f; - m_lastposition = _position; - - l_position.X = vec.X; - l_position.Y = vec.Y; - l_position.Z = vec.Z; - if (l_position.Z < 0) - { - // This is so prim that get lost underground don't fall forever and suck up - // - // Sim resources and memory. - // Disables the prim's movement physics.... - // It's a hack and will generate a console message if it fails. - - - - - //IsPhysical = false; - base.RaiseOutOfBounds(_position); - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - base.RequestPhysicsterseUpdate(); - m_throttleUpdates = false; - throttleCounter = 0; - _zeroFlag = true; - //outofBounds = true; - } - - if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02 )) - { - - _zeroFlag = true; - } - else - { - //System.Console.WriteLine(Math.Abs(m_lastposition.X - l_position.X).ToString()); - _zeroFlag = false; - } - - - - if (_zeroFlag) - { - // Supposedly this is supposed to tell SceneObjectGroup that - // no more updates need to be sent.. - // but it seems broken. - _velocity.X = 0.0f; - _velocity.Y = 0.0f; - _velocity.Z = 0.0f; - //_orientation.w = 0f; - //_orientation.x = 0f; - //_orientation.y = 0f; - //_orientation.z = 0f; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - if (!m_lastUpdateSent) - { - m_throttleUpdates = false; - throttleCounter = 0; - base.RequestPhysicsterseUpdate(); - m_lastUpdateSent = true; - } - - } - else - { - m_lastVelocity = _velocity; - - _position = l_position; - - _velocity.X = vel.X; - _velocity.Y = vel.Y; - _velocity.Z = vel.Z; - - m_rotationalVelocity.X = rotvel.X; - m_rotationalVelocity.Y = rotvel.Y; - m_rotationalVelocity.Z = rotvel.Z; - //System.Console.WriteLine("ODE: " + m_rotationalVelocity.ToString()); - _orientation.w = ori.W; - _orientation.x = ori.X; - _orientation.y = ori.Y; - _orientation.z = ori.Z; - m_lastUpdateSent = false; - if (!m_throttleUpdates || throttleCounter > 15) - { - base.RequestPhysicsterseUpdate(); - } - else - { - throttleCounter++; - } - } - m_lastposition = l_position; - } - else - { - // Not a body.. so Make sure the client isn't interpolating - _velocity.X = 0; - _velocity.Y = 0; - _velocity.Z = 0; - m_rotationalVelocity.X = 0; - m_rotationalVelocity.Y = 0; - m_rotationalVelocity.Z = 0; - _zeroFlag = true; - } - - } - public override void SetMomentum(PhysicsVector momentum) - { - } - } -} - #endregion \ No newline at end of file + +} \ No newline at end of file