From 827b0fb1993c6f9b1289931a1ac38ff2b810952c Mon Sep 17 00:00:00 2001 From: opensim Date: Wed, 30 Sep 2009 18:51:02 +0200 Subject: Commit initial version of KittoFlora's vehicle changes --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 594 +++++++++++++++------------- 1 file changed, 323 insertions(+), 271 deletions(-) (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 673ae39..34844c0 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -1,4 +1,15 @@ /* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. * @@ -72,6 +83,9 @@ namespace OpenSim.Region.Physics.OdePlugin private float PID_G = 25f; private bool m_usePID = false; + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + private float m_PIDHoverHeight = 0f; private float m_PIDHoverTau = 0f; private bool m_useHoverPID = false; @@ -79,6 +93,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_targetHoverHeight = 0f; private float m_groundHeight = 0f; private float m_waterHeight = 0f; + private float m_buoyancy = 0f; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -147,8 +162,6 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_roundsUnderMotionThreshold = 0; private int m_crossingfailures = 0; - public float m_buoyancy = 0f; - public bool outofBounds = false; private float m_density = 10.000006836f; // Aluminum g/cm3; @@ -156,7 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_lastUpdateSent = false; public IntPtr Body = (IntPtr) 0; - private String m_primName; + public String m_primName; +// private String m_primName; private PhysicsVector _target_velocity; public d.Mass pMass; @@ -167,7 +181,7 @@ namespace OpenSim.Region.Physics.OdePlugin public volatile bool childPrim = false; - private ODEVehicleSettings m_vehicle; + private ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; @@ -175,7 +189,7 @@ namespace OpenSim.Region.Physics.OdePlugin Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) { _target_velocity = new PhysicsVector(0, 0, 0); - m_vehicle = new ODEVehicleSettings(); + m_vehicle = new ODEDynamics(); //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); ode = dode; _velocity = new PhysicsVector(); @@ -260,11 +274,6 @@ namespace OpenSim.Region.Physics.OdePlugin m_localID = value; } } - public override int GetHashCode() - { - return (int)m_localID; - } - public override bool Grabbed { set { return; } @@ -273,6 +282,8 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool Selected { set { + + // This only makes the object not collidable if the object // is physical or the object is modified somehow *IN THE FUTURE* // without this, if an avatar selects prim, they can walk right @@ -288,6 +299,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } + if(m_isSelected) disableBodySoft(); } } @@ -295,6 +307,7 @@ namespace OpenSim.Region.Physics.OdePlugin { prev_geom = prim_geom; prim_geom = geom; +//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); @@ -306,6 +319,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null && _parent is OdePrim) { OdePrim parent = (OdePrim)_parent; +//Console.WriteLine("SetGeom calls ChildSetGeom"); parent.ChildSetGeom(this); } } @@ -321,7 +335,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical && Body != IntPtr.Zero) { d.BodyEnable(Body); - m_vehicle.Enable(Body, _parent_scene); + if (m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Enable(Body, _parent_scene); } m_disabled = false; @@ -335,7 +350,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical && Body != IntPtr.Zero) { d.BodyDisable(Body); - m_vehicle.Disable(); } } @@ -365,6 +379,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode (Body, false); m_interpenetrationcount = 0; m_collisionscore = 0; @@ -711,13 +728,8 @@ namespace OpenSim.Region.Physics.OdePlugin break; } } - - - - - return returnMass; - } + }// end CalculateMass #endregion @@ -743,7 +755,6 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { _parent_scene.remActivePrim(this); - m_vehicle.Destroy(); m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); @@ -838,6 +849,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prim_geom == IntPtr.Zero) { +//Console.WriteLine(" setMesh 1"); SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); } } @@ -865,19 +877,35 @@ namespace OpenSim.Region.Physics.OdePlugin public void ProcessTaints(float timestep) { +//Console.WriteLine("ProcessTaints for " + m_primName ); if (m_taintadd) { changeadd(timestep); } + if (prim_geom != IntPtr.Zero) { - if (!_position.IsIdentical(m_taintposition,0f)) - changemove(timestep); - - if (m_taintrot != _orientation) - rotate(timestep); + if (!_position.IsIdentical(m_taintposition,0f)) + changemove(timestep); + + if (m_taintrot != _orientation) + { + if(childPrim && IsPhysical) // For physical child prim... + { + rotate(timestep); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(timestep); + } + else + { + //Just rotate the prim + rotate(timestep); + } + } // - + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) changePhysicsStatus(timestep); // @@ -916,8 +944,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!m_angularlock.IsIdentical(m_taintAngularLock,0)) changeAngularLock(timestep); - - + } else { @@ -949,11 +976,6 @@ namespace OpenSim.Region.Physics.OdePlugin Amotor = IntPtr.Zero; } } - - if (m_vehicle.Type != Vehicle.TYPE_NONE) - { - m_vehicle.Reset(); - } } } // Store this for later in case we get turned into a separate body @@ -971,7 +993,7 @@ namespace OpenSim.Region.Physics.OdePlugin { OdePrim obj = (OdePrim)m_taintparent; //obj.disableBody(); - +//Console.WriteLine("changelink calls ParentPrim"); obj.ParentPrim(this); /* @@ -989,6 +1011,8 @@ namespace OpenSim.Region.Physics.OdePlugin // destroy link else if (_parent != null && m_taintparent == null) { +//Console.WriteLine(" changelink B"); + if (_parent is OdePrim) { OdePrim obj = (OdePrim)_parent; @@ -1005,7 +1029,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linkJoint = (IntPtr)0; */ } - + _parent = m_taintparent; m_taintPhysics = m_isphysical; } @@ -1014,6 +1038,7 @@ namespace OpenSim.Region.Physics.OdePlugin // prim is the child public void ParentPrim(OdePrim prim) { +//Console.WriteLine("ParentPrim " + m_primName); if (this.m_localID != prim.m_localID) { if (Body == IntPtr.Zero) @@ -1027,6 +1052,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childrenPrim.Contains(prim)) { +//Console.WriteLine("childrenPrim.Add " + prim); childrenPrim.Add(prim); foreach (OdePrim prm in childrenPrim) @@ -1050,6 +1076,7 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { + prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); @@ -1058,7 +1085,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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); @@ -1103,11 +1130,12 @@ namespace OpenSim.Region.Physics.OdePlugin 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); @@ -1143,7 +1171,7 @@ namespace OpenSim.Region.Physics.OdePlugin createAMotor(m_angularlock); } d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); - m_vehicle.Enable(Body, _parent_scene); + if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); _parent_scene.addActivePrim(this); } } @@ -1180,6 +1208,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { +//Console.WriteLine("ChildSetGeom calls ParentPrim"); ParentPrim(prm); } } @@ -1206,6 +1235,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (childrenPrim) { + //Console.WriteLine("childrenPrim.Remove " + odePrim); childrenPrim.Remove(odePrim); } @@ -1223,6 +1253,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { +//Console.WriteLine("ChildDelink calls ParentPrim"); ParentPrim(prm); } } @@ -1307,7 +1338,7 @@ namespace OpenSim.Region.Physics.OdePlugin resetCollisionAccounting(); m_isSelected = m_taintselected; - } + }//end changeSelectedStatus public void ResetTaints() { @@ -1324,6 +1355,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { +//Console.WriteLine("CreateGeom:"); if (_mesh != null) { setMesh(_parent_scene, _mesh); @@ -1339,6 +1371,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { +//Console.WriteLine(" CreateGeom 1"); SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); } catch (AccessViolationException) @@ -1353,6 +1386,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { +//Console.WriteLine(" CreateGeom 2"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1368,6 +1402,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { +//Console.WriteLine(" CreateGeom 3"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1384,6 +1419,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { +//Console.WriteLine(" CreateGeom 4"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1420,6 +1456,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (_parent_scene.OdeLock) { +//Console.WriteLine("changeadd 1"); CreateGeom(m_targetSpace, _mesh); if (prim_geom != IntPtr.Zero) @@ -1475,6 +1512,8 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim odParent = (OdePrim)_parent; if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) { +// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? +Console.WriteLine(" JointCreateFixed"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1528,239 +1567,236 @@ namespace OpenSim.Region.Physics.OdePlugin float fz = 0; - if (IsPhysical && Body != IntPtr.Zero && !m_isSelected) - { - if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) - { - d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularlock.X == 1) - avel2.X = 0; - if (m_angularlock.Y == 1) - avel2.Y = 0; - if (m_angularlock.Z == 1) - avel2.Z = 0; - d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); - } - //float PID_P = 900.0f; - - float m_mass = CalculateMass(); - - fz = 0f; + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + { + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(timestep, _parent_scene); + } + else + { + // NON-'VEHICLES' are dealt with here + if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) + { + d.Vector3 avel2 = d.BodyGetAngularVel(Body); + if (m_angularlock.X == 1) + avel2.X = 0; + if (m_angularlock.Y == 1) + avel2.Y = 0; + if (m_angularlock.Z == 1) + avel2.Z = 0; + d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); + } + //float PID_P = 900.0f; + + float m_mass = CalculateMass(); + +// fz = 0f; //m_log.Info(m_collisionFlags.ToString()); - if (m_buoyancy != 0) - { - if (m_buoyancy > 0) - { - fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass); - - //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); - //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); - } - else - { - fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass)); - } - } - - if (m_usePID) - { + + //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // gravityz multiplier = 1 - m_buoyancy + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; + + if (m_usePID) + { + // KF - this is for object move? eg. llSetPos() ? + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); + // If we're using the PID controller, then we have no gravity + //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... + fz = 0f; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + //PidStatus = true; + + // PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new PhysicsVector( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + if (m_useHoverPID && !m_usePID) + { + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - // no lock; for now it's only called from within Simulate() + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new PhysicsVector(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } + + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; - - // PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = - new PhysicsVector( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !m_usePID) - { - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Absolute: - m_targetHoverHeight = m_PIDHoverHeight; - break; - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - case PIDHoverType.Water: - m_waterHeight = _parent_scene.GetWaterLevel(); - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - break; - } - - - _target_velocity = - new PhysicsVector(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } - - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; - - - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); - } - if (m_vehicle.Body == IntPtr.Zero && m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); - - m_vehicle.Step(timestep); + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); + } + } } else - { - // _zeroPosition = d.BodyGetPosition(Body); + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); return; } } @@ -1774,14 +1810,22 @@ namespace OpenSim.Region.Physics.OdePlugin myrot.Y = _orientation.Y; myrot.Z = _orientation.Z; myrot.W = _orientation.W; - d.GeomSetQuaternion(prim_geom, ref myrot); - if (m_isphysical && Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { + // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0)) - createAMotor(m_angularlock); + if (m_isphysical) + { + if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0)) + createAMotor(m_angularlock); + } + } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); } - + resetCollisionAccounting(); m_taintrot = _orientation; } @@ -1843,7 +1887,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Error("[PHYSICS]: PrimGeom dead"); } } - +//Console.WriteLine("changePhysicsStatus for " + m_primName ); changeadd(2f); } if (childPrim) @@ -1921,7 +1965,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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); @@ -1929,6 +1973,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { _mesh = null; +//Console.WriteLine("changesize 2"); CreateGeom(m_targetSpace, _mesh); } @@ -2035,6 +2080,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { _mesh = null; +//Console.WriteLine("changeshape"); CreateGeom(m_targetSpace, null); } @@ -2376,7 +2422,9 @@ namespace OpenSim.Region.Physics.OdePlugin set { if (QuaternionIsFinite(value)) + { _orientation = value; + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); @@ -2595,12 +2643,16 @@ namespace OpenSim.Region.Physics.OdePlugin //outofBounds = true; } + float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); 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) - && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) +// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large { _zeroFlag = true; +//Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else -- cgit v1.1