From 827b0fb1993c6f9b1289931a1ac38ff2b810952c Mon Sep 17 00:00:00 2001 From: opensim Date: Wed, 30 Sep 2009 18:51:02 +0200 Subject: [PATCH] Commit initial version of KittoFlora's vehicle changes --- .../Examples/SimpleModule/ComplexObject.cs | 6 +- .../Framework/Scenes/Scene.Inventory.cs | 2 +- OpenSim/Region/Framework/Scenes/Scene.cs | 4 +- OpenSim/Region/Framework/Scenes/SceneGraph.cs | 6 +- .../Framework/Scenes/SceneObjectGroup.cs | 46 +- .../Framework/Scenes/SceneObjectPart.cs | 267 +++---- .../Scenes/Tests/SceneObjectLinkingTests.cs | 6 +- ...icleSettings.cs => ODEDynamics.c_comments} | 552 +++++++-------- .../Region/Physics/OdePlugin/ODEDynamics.cs | 658 ++++++++++++++++++ OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 534 +++++++------- OpenSim/Region/Physics/OdePlugin/OdePlugin.cs | 52 +- .../Shared/Api/Implementation/LSL_Api.cs | 31 +- 12 files changed, 1439 insertions(+), 725 deletions(-) rename OpenSim/Region/Physics/OdePlugin/{ODEVehicleSettings.cs => ODEDynamics.c_comments} (62%) create mode 100644 OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs diff --git a/OpenSim/Region/Examples/SimpleModule/ComplexObject.cs b/OpenSim/Region/Examples/SimpleModule/ComplexObject.cs index 3809749fa5..e951befdc5 100644 --- a/OpenSim/Region/Examples/SimpleModule/ComplexObject.cs +++ b/OpenSim/Region/Examples/SimpleModule/ComplexObject.cs @@ -68,11 +68,15 @@ namespace OpenSim.Region.Examples.SimpleModule public override void UpdateMovement() { - UpdateGroupRotation(GroupRotation * m_rotationDirection); + UpdateGroupRotationR(GroupRotation * m_rotationDirection); base.UpdateMovement(); } + public ComplexObject() + { + } + public ComplexObject(Scene scene, ulong regionHandle, UUID ownerID, uint localID, Vector3 pos) : base(ownerID, pos, PrimitiveBaseShape.Default) { diff --git a/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs b/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs index eb397f68fd..a6e137d581 100644 --- a/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.Inventory.cs @@ -2265,7 +2265,7 @@ namespace OpenSim.Region.Framework.Scenes group.ClearPartAttachmentData(); } - group.UpdateGroupRotation(rot); + group.UpdateGroupRotationR(rot); //group.ApplyPhysics(m_physicalPrim); if (group.RootPart.PhysActor != null && group.RootPart.PhysActor.IsPhysical && vel != Vector3.Zero) diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs index d8478a24f2..db107a91a7 100644 --- a/OpenSim/Region/Framework/Scenes/Scene.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.cs @@ -2810,7 +2810,7 @@ namespace OpenSim.Region.Framework.Scenes // SceneObjectGroup obj = m_sceneGraph.DuplicateObject(localID, pos, target.GetEffectiveObjectFlags(), AgentID, GroupID, worldRot); m_sceneGraph.DuplicateObject(localID, pos, target.GetEffectiveObjectFlags(), AgentID, GroupID, worldRot); //obj.Rotation = worldRot; - //obj.UpdateGroupRotation(worldRot); + //obj.UpdateGroupRotationR(worldRot); } else { @@ -4504,7 +4504,7 @@ namespace OpenSim.Region.Framework.Scenes Quaternion q = trackedBody.RotationOffset * joint.LocalRotation; jointProxyObject.ParentGroup.UpdateGroupPosition(proxyPos); // schedules the entire group for a terse update - jointProxyObject.ParentGroup.UpdateGroupRotation(q); // schedules the entire group for a terse update + jointProxyObject.ParentGroup.UpdateGroupRotationR(q); // schedules the entire group for a terse update } break; } diff --git a/OpenSim/Region/Framework/Scenes/SceneGraph.cs b/OpenSim/Region/Framework/Scenes/SceneGraph.cs index 48dea0719a..09116fa1a6 100644 --- a/OpenSim/Region/Framework/Scenes/SceneGraph.cs +++ b/OpenSim/Region/Framework/Scenes/SceneGraph.cs @@ -1236,7 +1236,7 @@ namespace OpenSim.Region.Framework.Scenes { if (m_parentScene.Permissions.CanMoveObject(group.UUID, remoteClient.AgentId)) { - group.UpdateGroupRotation(rot); + group.UpdateGroupRotationR(rot); } } } @@ -1255,7 +1255,7 @@ namespace OpenSim.Region.Framework.Scenes { if (m_parentScene.Permissions.CanMoveObject(group.UUID, remoteClient.AgentId)) { - group.UpdateGroupRotation(pos, rot); + group.UpdateGroupRotationPR(pos, rot); } } } @@ -1811,7 +1811,7 @@ namespace OpenSim.Region.Framework.Scenes if (rot != Quaternion.Identity) { - copy.UpdateGroupRotation(rot); + copy.UpdateGroupRotationR(rot); } copy.CreateScriptInstances(0, false, m_parentScene.DefaultScriptEngine, 0); diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 3c17bbe9dd..4bd10bdf40 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -367,6 +367,13 @@ namespace OpenSim.Region.Framework.Scenes #region Constructors + /// + /// Constructor + /// + public SceneObjectGroup() + { + } + /// /// This constructor creates a SceneObjectGroup using a pre-existing SceneObjectPart. /// The original SceneObjectPart will be used rather than a copy, preserving @@ -2953,8 +2960,9 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void UpdateGroupRotation(Quaternion rot) + public void UpdateGroupRotationR(Quaternion rot) { + m_rootPart.UpdateRotation(rot); if (m_rootPart.PhysActor != null) { @@ -2971,7 +2979,7 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void UpdateGroupRotation(Vector3 pos, Quaternion rot) + public void UpdateGroupRotationPR(Vector3 pos, Quaternion rot) { m_rootPart.UpdateRotation(rot); if (m_rootPart.PhysActor != null) @@ -3079,22 +3087,26 @@ namespace OpenSim.Region.Framework.Scenes int yaxis = 4; int zaxis = 8; - setX = ((axis & xaxis) != 0) ? true : false; - setY = ((axis & yaxis) != 0) ? true : false; - setZ = ((axis & zaxis) != 0) ? true : false; - - float setval = (rotate10 > 0) ? 1f : 0f; - - if (setX) - m_rootPart.RotationAxis.X = setval; - if (setY) - m_rootPart.RotationAxis.Y = setval; - if (setZ) - m_rootPart.RotationAxis.Z = setval; - - if (setX || setY || setZ) + if (m_rootPart != null) { - m_rootPart.SetPhysicsAxisRotation(); + setX = ((axis & xaxis) != 0) ? true : false; + setY = ((axis & yaxis) != 0) ? true : false; + setZ = ((axis & zaxis) != 0) ? true : false; + + float setval = (rotate10 > 0) ? 1f : 0f; + + if (setX) + m_rootPart.RotationAxis.X = setval; + if (setY) + m_rootPart.RotationAxis.Y = setval; + if (setZ) + m_rootPart.RotationAxis.Z = setval; + + if (setX || setY || setZ) + { + m_rootPart.SetPhysicsAxisRotation(); + } + } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 51bb1145dd..40e7471268 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -415,10 +415,9 @@ namespace OpenSim.Region.Framework.Scenes set { m_name = value; - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.SOPName = value; + PhysActor.SOPName = value; } } } @@ -428,11 +427,10 @@ namespace OpenSim.Region.Framework.Scenes get { return (byte) m_material; } set { - PhysicsActor pa = PhysActor; m_material = (Material)value; - if (pa != null) + if (PhysActor != null) { - pa.SetMaterial((int)value); + PhysActor.SetMaterial((int)value); } } } @@ -503,12 +501,11 @@ namespace OpenSim.Region.Framework.Scenes get { // If this is a linkset, we don't want the physics engine mucking up our group position here. - PhysicsActor pa = PhysActor; - if (pa != null && _parentID == 0) + if (PhysActor != null && _parentID == 0) { - m_groupPosition.X = pa.Position.X; - m_groupPosition.Y = pa.Position.Y; - m_groupPosition.Z = pa.Position.Z; + m_groupPosition.X = PhysActor.Position.X; + m_groupPosition.Y = PhysActor.Position.Y; + m_groupPosition.Z = PhysActor.Position.Z; } if (IsAttachment) @@ -528,27 +525,26 @@ namespace OpenSim.Region.Framework.Scenes m_groupPosition = value; - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { try { // Root prim actually goes at Position if (_parentID == 0) { - pa.Position = new PhysicsVector(value.X, value.Y, value.Z); + PhysActor.Position = new PhysicsVector(value.X, value.Y, value.Z); } else { // To move the child prim in respect to the group position and rotation we have to calculate Vector3 resultingposition = GetWorldPosition(); - pa.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z); + PhysActor.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z); Quaternion resultingrot = GetWorldRotation(); - pa.Orientation = resultingrot; + PhysActor.Orientation = resultingrot; } // Tell the physics engines that this prim changed. - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } catch (Exception e) { @@ -581,16 +577,15 @@ namespace OpenSim.Region.Framework.Scenes if (ParentGroup != null && !ParentGroup.IsDeleted) { - PhysicsActor pa = PhysActor; - if (_parentID != 0 && pa != null) + if (_parentID != 0 && PhysActor != null) { Vector3 resultingposition = GetWorldPosition(); - pa.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z); + PhysActor.Position = new PhysicsVector(resultingposition.X, resultingposition.Y, resultingposition.Z); Quaternion resultingrot = GetWorldRotation(); - pa.Orientation = resultingrot; + PhysActor.Orientation = resultingrot; // Tell the physics engines that this prim changed. - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } } @@ -600,14 +595,13 @@ namespace OpenSim.Region.Framework.Scenes { get { - PhysicsActor pa = PhysActor; // We don't want the physics engine mucking up the rotations in a linkset - if ((_parentID == 0) && (Shape.PCode != 9 || Shape.State == 0) && (pa != null)) + if ((_parentID == 0) && (Shape.PCode != 9 || Shape.State == 0) && (PhysActor != null)) { - if (pa.Orientation.X != 0 || pa.Orientation.Y != 0 - || pa.Orientation.Z != 0 || pa.Orientation.W != 0) + if (PhysActor.Orientation.X != 0 || PhysActor.Orientation.Y != 0 + || PhysActor.Orientation.Z != 0 || PhysActor.Orientation.W != 0) { - m_rotationOffset = pa.Orientation; + m_rotationOffset = PhysActor.Orientation; } } @@ -616,28 +610,27 @@ namespace OpenSim.Region.Framework.Scenes set { - PhysicsActor pa = PhysActor; StoreUndoState(); m_rotationOffset = value; - if (pa != null) + if (PhysActor != null) { try { // Root prim gets value directly if (_parentID == 0) { - pa.Orientation = value; + PhysActor.Orientation = value; //m_log.Info("[PART]: RO1:" + PhysActor.Orientation.ToString()); } else { // Child prim we have to calculate it's world rotationwel Quaternion resultingrotation = GetWorldRotation(); - pa.Orientation = resultingrotation; + PhysActor.Orientation = resultingrotation; //m_log.Info("[PART]: RO2:" + PhysActor.Orientation.ToString()); } - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); //} } catch (Exception ex) @@ -657,14 +650,13 @@ namespace OpenSim.Region.Framework.Scenes //if (PhysActor.Velocity.X != 0 || PhysActor.Velocity.Y != 0 //|| PhysActor.Velocity.Z != 0) //{ - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - if (pa.IsPhysical) + if (PhysActor.IsPhysical) { - m_velocity.X = pa.Velocity.X; - m_velocity.Y = pa.Velocity.Y; - m_velocity.Z = pa.Velocity.Z; + m_velocity.X = PhysActor.Velocity.X; + m_velocity.Y = PhysActor.Velocity.Y; + m_velocity.Z = PhysActor.Velocity.Z; } } @@ -674,13 +666,12 @@ namespace OpenSim.Region.Framework.Scenes set { m_velocity = value; - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - if (pa.IsPhysical) + if (PhysActor.IsPhysical) { - pa.Velocity = new PhysicsVector(value.X, value.Y, value.Z); - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + PhysActor.Velocity = new PhysicsVector(value.X, value.Y, value.Z); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } } @@ -697,10 +688,9 @@ namespace OpenSim.Region.Framework.Scenes { get { - PhysicsActor pa = PhysActor; - if ((pa != null) && pa.IsPhysical) + if ((PhysActor != null) && PhysActor.IsPhysical) { - m_angularVelocity.FromBytes(pa.RotationalVelocity.GetBytes(), 0); + m_angularVelocity.FromBytes(PhysActor.RotationalVelocity.GetBytes(), 0); } return m_angularVelocity; } @@ -719,11 +709,10 @@ namespace OpenSim.Region.Framework.Scenes get { return m_description; } set { - PhysicsActor pa = PhysActor; m_description = value; - if (pa != null) + if (PhysActor != null) { - pa.SOPDescription = value; + PhysActor.SOPDescription = value; } } } @@ -817,15 +806,14 @@ namespace OpenSim.Region.Framework.Scenes if (m_shape != null) { m_shape.Scale = value; - PhysicsActor pa = PhysActor; - if (pa != null && m_parentGroup != null) + if (PhysActor != null && m_parentGroup != null) { if (m_parentGroup.Scene != null) { if (m_parentGroup.Scene.PhysicsScene != null) { - pa.Size = new PhysicsVector(m_shape.Scale.X, m_shape.Scale.Y, m_shape.Scale.Z); - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + PhysActor.Size = new PhysicsVector(m_shape.Scale.X, m_shape.Scale.Y, m_shape.Scale.Z); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } } @@ -1355,14 +1343,13 @@ if (m_shape != null) { RigidBody); // Basic Physics returns null.. joy joy joy. - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.SOPName = this.Name; // save object name and desc into the PhysActor so ODE internals know the joint/body info - pa.SOPDescription = this.Description; - pa.LocalID = LocalId; + PhysActor.SOPName = this.Name; // save object name and desc into the PhysActor so ODE internals know the joint/body info + PhysActor.SOPDescription = this.Description; + PhysActor.LocalID = LocalId; DoPhysicsPropertyUpdate(RigidBody, true); - pa.SetVolumeDetect(VolumeDetectActive ? 1 : 0); + PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0); } } } @@ -1576,24 +1563,23 @@ if (m_shape != null) { } else { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - if (UsePhysics != pa.IsPhysical || isNew) + if (UsePhysics != PhysActor.IsPhysical || isNew) { - if (pa.IsPhysical) // implies UsePhysics==false for this block + if (PhysActor.IsPhysical) // implies UsePhysics==false for this block { if (!isNew) ParentGroup.Scene.RemovePhysicalPrim(1); - pa.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate; - pa.OnOutOfBounds -= PhysicsOutOfBounds; - pa.delink(); + PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate; + PhysActor.OnOutOfBounds -= PhysicsOutOfBounds; + PhysActor.delink(); if (ParentGroup.Scene.PhysicsScene.SupportsNINJAJoints && (!isNew)) { // destroy all joints connected to this now deactivated body - m_parentGroup.Scene.PhysicsScene.RemoveAllJointsConnectedToActorThreadLocked(pa); + m_parentGroup.Scene.PhysicsScene.RemoveAllJointsConnectedToActorThreadLocked(PhysActor); } // stop client-side interpolation of all joint proxy objects that have just been deleted @@ -1612,7 +1598,7 @@ if (m_shape != null) { //RotationalVelocity = new Vector3(0, 0, 0); } - pa.IsPhysical = UsePhysics; + PhysActor.IsPhysical = UsePhysics; // If we're not what we're supposed to be in the physics scene, recreate ourselves. @@ -1626,19 +1612,19 @@ if (m_shape != null) { { ParentGroup.Scene.AddPhysicalPrim(1); - pa.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate; - pa.OnOutOfBounds += PhysicsOutOfBounds; + PhysActor.OnRequestTerseUpdate += PhysicsRequestingTerseUpdate; + PhysActor.OnOutOfBounds += PhysicsOutOfBounds; if (_parentID != 0 && _parentID != LocalId) { if (ParentGroup.RootPart.PhysActor != null) { - pa.link(ParentGroup.RootPart.PhysActor); + PhysActor.link(ParentGroup.RootPart.PhysActor); } } } } } - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } } @@ -1704,10 +1690,9 @@ if (m_shape != null) { public Vector3 GetGeometricCenter() { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - return new Vector3(pa.CenterOfMass.X, pa.CenterOfMass.Y, pa.CenterOfMass.Z); + return new Vector3(PhysActor.CenterOfMass.X, PhysActor.CenterOfMass.Y, PhysActor.CenterOfMass.Z); } else { @@ -1717,10 +1702,9 @@ if (m_shape != null) { public float GetMass() { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - return pa.Mass; + return PhysActor.Mass; } else { @@ -1730,9 +1714,8 @@ if (m_shape != null) { public PhysicsVector GetForce() { - PhysicsActor pa = PhysActor; - if (pa != null) - return pa.Force; + if (PhysActor != null) + return PhysActor.Force; else return new PhysicsVector(); } @@ -2111,15 +2094,11 @@ if (m_shape != null) { public void PhysicsRequestingTerseUpdate() { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - Vector3 newpos = new Vector3(pa.Position.GetBytes(), 0); + Vector3 newpos = new Vector3(PhysActor.Position.GetBytes(), 0); - if (m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.N) | - m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.S) | - m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.E) | - m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.W)) + if (m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.N) | m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.S) | m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.E) | m_parentGroup.Scene.TestBorderCross(newpos, Cardinals.W)) { m_parentGroup.AbsolutePosition = newpos; return; @@ -2315,15 +2294,14 @@ if (m_shape != null) { if (texture != null) m_shape.SculptData = texture.Data; - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { // Tricks physics engine into thinking we've changed the part shape. PrimitiveBaseShape m_newshape = m_shape.Copy(); - pa.Shape = m_newshape; + PhysActor.Shape = m_newshape; m_shape = m_newshape; - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } } @@ -2542,10 +2520,9 @@ if (m_shape != null) { public void SetBuoyancy(float fvalue) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.Buoyancy = fvalue; + PhysActor.Buoyancy = fvalue; } } @@ -2561,62 +2538,56 @@ if (m_shape != null) { public void SetFloatOnWater(int floatYN) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { if (floatYN == 1) { - pa.FloatOnWater = true; + PhysActor.FloatOnWater = true; } else { - pa.FloatOnWater = false; + PhysActor.FloatOnWater = false; } } } public void SetForce(PhysicsVector force) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.Force = force; + PhysActor.Force = force; } } public void SetVehicleType(int type) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.VehicleType = type; + PhysActor.VehicleType = type; } } public void SetVehicleFloatParam(int param, float value) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.VehicleFloatParam(param, value); + PhysActor.VehicleFloatParam(param, value); } } public void SetVehicleVectorParam(int param, PhysicsVector value) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.VehicleVectorParam(param, value); + PhysActor.VehicleVectorParam(param, value); } } public void SetVehicleRotationParam(int param, Quaternion rotation) { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.VehicleRotationParam(param, rotation); + PhysActor.VehicleRotationParam(param, rotation); } } @@ -2644,11 +2615,10 @@ if (m_shape != null) { public void SetPhysicsAxisRotation() { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.LockAngularMotion(RotationAxis); - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + PhysActor.LockAngularMotion(RotationAxis); + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } } @@ -3380,9 +3350,8 @@ if (m_shape != null) { { IsVD = false; // Switch it of for the course of this routine VolumeDetectActive = false; // and also permanently - PhysicsActor pa = PhysActor; - if (pa != null) - pa.SetVolumeDetect(0); // Let physics know about it too + if (PhysActor != null) + PhysActor.SetVolumeDetect(0); // Let physics know about it too } else { @@ -3430,21 +3399,18 @@ if (m_shape != null) { if (IsPhantom || IsAttachment) // note: this may have been changed above in the case of joints { AddFlag(PrimFlags.Phantom); - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - m_parentGroup.Scene.PhysicsScene.RemovePrim(pa); + m_parentGroup.Scene.PhysicsScene.RemovePrim(PhysActor); /// that's not wholesome. Had to make Scene public - pa = null; + PhysActor = null; } } else // Not phantom { RemFlag(PrimFlags.Phantom); - // This is NOT safe!! - PhysicsActor pa = PhysActor; - if (pa == null) + if (PhysActor == null) { // It's not phantom anymore. So make sure the physics engine get's knowledge of it PhysActor = m_parentGroup.Scene.PhysicsScene.AddPrimShape( @@ -3455,9 +3421,9 @@ if (m_shape != null) { RotationOffset, UsePhysics); - if (pa != null) + if (PhysActor != null) { - pa.LocalID = LocalId; + PhysActor.LocalID = LocalId; DoPhysicsPropertyUpdate(UsePhysics, true); if (m_parentGroup != null) { @@ -3476,14 +3442,14 @@ if (m_shape != null) { (CollisionSound != UUID.Zero) ) { - pa.OnCollisionUpdate += PhysicsCollision; - pa.SubscribeEvents(1000); + PhysActor.OnCollisionUpdate += PhysicsCollision; + PhysActor.SubscribeEvents(1000); } } } else // it already has a physical representation { - pa.IsPhysical = UsePhysics; + PhysActor.IsPhysical = UsePhysics; DoPhysicsPropertyUpdate(UsePhysics, false); // Update physical status. If it's phantom this will remove the prim if (m_parentGroup != null) @@ -3506,10 +3472,9 @@ if (m_shape != null) { // Defensive programming calls for a check here. // Better would be throwing an exception that could be catched by a unit test as the internal // logic should make sure, this Physactor is always here. - PhysicsActor pa = this.PhysActor; - if (pa != null) + if (this.PhysActor != null) { - pa.SetVolumeDetect(1); + PhysActor.SetVolumeDetect(1); AddFlag(PrimFlags.Phantom); // We set this flag also if VD is active this.VolumeDetectActive = true; } @@ -3517,11 +3482,10 @@ if (m_shape != null) { } else { // Remove VolumeDetect in any case. Note, it's safe to call SetVolumeDetect as often as you like - // (mumbles, well, at least if you have infinte CPU powers :-) ) - PhysicsActor pa = this.PhysActor; - if (pa != null) + // (mumbles, well, at least if you have infinte CPU powers :-)) + if (this.PhysActor != null) { - pa.SetVolumeDetect(0); + PhysActor.SetVolumeDetect(0); } this.VolumeDetectActive = false; } @@ -3579,11 +3543,10 @@ if (m_shape != null) { m_shape.PathTaperY = shapeBlock.PathTaperY; m_shape.PathTwist = shapeBlock.PathTwist; m_shape.PathTwistBegin = shapeBlock.PathTwistBegin; - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.Shape = m_shape; - m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(pa); + PhysActor.Shape = m_shape; + m_parentGroup.Scene.PhysicsScene.AddPhysicsActorTaint(PhysActor); } // This is what makes vehicle trailers work @@ -3684,21 +3647,19 @@ if (m_shape != null) { ) { // subscribe to physics updates. - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.OnCollisionUpdate += PhysicsCollision; - pa.SubscribeEvents(1000); + PhysActor.OnCollisionUpdate += PhysicsCollision; + PhysActor.SubscribeEvents(1000); } } else { - PhysicsActor pa = PhysActor; - if (pa != null) + if (PhysActor != null) { - pa.UnSubscribeEvents(); - pa.OnCollisionUpdate -= PhysicsCollision; + PhysActor.UnSubscribeEvents(); + PhysActor.OnCollisionUpdate -= PhysicsCollision; } } @@ -3801,8 +3762,6 @@ if (m_shape != null) { lPos = AbsolutePosition; } - // Causes this thread to dig into the Client Thread Data. - // Remember your locking here! remoteClient.SendPrimTerseUpdate(m_regionHandle, (ushort)(m_parentGroup.GetTimeDilation() * (float)ushort.MaxValue), LocalId, lPos, diff --git a/OpenSim/Region/Framework/Scenes/Tests/SceneObjectLinkingTests.cs b/OpenSim/Region/Framework/Scenes/Tests/SceneObjectLinkingTests.cs index bf1360736e..e74b18e3e1 100644 --- a/OpenSim/Region/Framework/Scenes/Tests/SceneObjectLinkingTests.cs +++ b/OpenSim/Region/Framework/Scenes/Tests/SceneObjectLinkingTests.cs @@ -70,7 +70,7 @@ namespace OpenSim.Region.Framework.Scenes.Tests grp1.Rotation = (Quaternion.CreateFromEulers(90 * Utils.DEG_TO_RAD, 0, 0)); // <180,0,0> - grp2.UpdateGroupRotation(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0)); + grp2.UpdateGroupRotationR(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0)); // Required for linking grp1.RootPart.UpdateFlag = 0; @@ -154,13 +154,13 @@ namespace OpenSim.Region.Framework.Scenes.Tests grp1.Rotation = (Quaternion.CreateFromEulers(90 * Utils.DEG_TO_RAD, 0, 0)); // <180,0,0> - grp2.UpdateGroupRotation(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0)); + grp2.UpdateGroupRotationR(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0)); // <270,0,0> grp3.Rotation = (Quaternion.CreateFromEulers(270 * Utils.DEG_TO_RAD, 0, 0)); // <0,90,0> - grp4.UpdateGroupRotation(Quaternion.CreateFromEulers(0, 90 * Utils.DEG_TO_RAD, 0)); + grp4.UpdateGroupRotationR(Quaternion.CreateFromEulers(0, 90 * Utils.DEG_TO_RAD, 0)); // Required for linking grp1.RootPart.UpdateFlag = 0; diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments similarity index 62% rename from OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs rename to OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments index a547c3e33c..1060aa69c6 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.c_comments @@ -1,4 +1,16 @@ /* + * 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. * @@ -37,10 +49,10 @@ using OpenSim.Region.Physics.Manager; namespace OpenSim.Region.Physics.OdePlugin { - public class ODEVehicleSettings + public class ODEDynamics { public Vehicle Type - { + { get { return m_type; } } @@ -49,49 +61,71 @@ namespace OpenSim.Region.Physics.OdePlugin get { return m_body; } } - private int frcount = 0; - // private float frmod = 3.0f; + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame - private Vehicle m_type = Vehicle.TYPE_NONE; // private OdeScene m_parentScene = null; private IntPtr m_body = IntPtr.Zero; private IntPtr m_jointGroup = IntPtr.Zero; private IntPtr m_aMotor = IntPtr.Zero; - private IntPtr m_lMotor1 = IntPtr.Zero; - // private IntPtr m_lMotor2 = IntPtr.Zero; - // private IntPtr m_lMotor3 = IntPtr.Zero; + // Vehicle properties - // private Quaternion m_referenceFrame = Quaternion.Identity; - private Vector3 m_angularFrictionTimescale = Vector3.Zero; + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; - private Vector3 m_linearFrictionTimescale = Vector3.Zero; - private Vector3 m_linearMotorDirection = Vector3.Zero; - private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; - // private Vector3 m_linearMotorOffset = Vector3.Zero; - // private float m_angularDeflectionEfficiency = 0; - // private float m_angularDeflectionTimescale = 0; + private Vector3 m_angularFrictionTimescale = Vector3.Zero; private float m_angularMotorDecayTimescale = 0; private float m_angularMotorTimescale = 0; + private Vector3 m_lastAngularVelocityVector = Vector3.Zero; + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties // private float m_bankingEfficiency = 0; // private float m_bankingMix = 0; // private float m_bankingTimescale = 0; - // private float m_buoyancy = 0; - // private float m_hoverHeight = 0; - // private float m_hoverEfficiency = 0; - // private float m_hoverTimescale = 0; - // private float m_linearDeflectionEfficiency = 0; - // private float m_linearDeflectionTimescale = 0; - private float m_linearMotorDecayTimescale = 0; - private float m_linearMotorTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties private float m_verticalAttractionEfficiency = 0; private float m_verticalAttractionTimescale = 0; - private Vector3 m_lastLinearVelocityVector = Vector3.Zero; - private Vector3 m_lastAngularVelocityVector = Vector3.Zero; - private VehicleFlag m_flags = (VehicleFlag) 0; - - // private bool m_LinearMotorSetLastFrame = false; + @@ -129,17 +163,21 @@ namespace OpenSim.Region.Physics.OdePlugin // m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: - // m_buoyancy = pValue; + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; break; case Vehicle.HOVER_EFFICIENCY: - // m_hoverEfficiency = pValue; + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; break; case Vehicle.HOVER_HEIGHT: - // m_hoverHeight = pValue; + m_VhoverHeight = pValue; break; case Vehicle.HOVER_TIMESCALE: if (pValue < 0.01f) pValue = 0.01f; - // m_hoverTimescale = pValue; + m_VhoverTimescale = pValue; break; case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: if (pValue < 0.01f) pValue = 0.01f; @@ -158,7 +196,8 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearMotorTimescale = pValue; break; case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0.01f) pValue = 0.01f; + if (pValue < 0.0f) pValue = 0.0f; + if (pValue > 1.0f) pValue = 1.0f; m_verticalAttractionEfficiency = pValue; break; case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: @@ -187,8 +226,8 @@ namespace OpenSim.Region.Physics.OdePlugin break; } - Reset(); - } + + }//end ProcessFloatVehicleParam internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) { @@ -212,8 +251,8 @@ namespace OpenSim.Region.Physics.OdePlugin // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); break; } - Reset(); - } + + }//end ProcessVectorVehicleParam internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) { @@ -223,113 +262,14 @@ namespace OpenSim.Region.Physics.OdePlugin // m_referenceFrame = pValue; break; } - Reset(); - } + + }//end ProcessRotationVehicleParam internal void ProcessTypeChange(Vehicle pType) { - if (m_type == Vehicle.TYPE_NONE && pType != Vehicle.TYPE_NONE) - { - // Activate whatever it is - SetDefaultsForType(pType); - Reset(); - } - else if (m_type != Vehicle.TYPE_NONE && pType != Vehicle.TYPE_NONE) - { - // Set properties - SetDefaultsForType(pType); - // then reset - Reset(); - } - else if (m_type != Vehicle.TYPE_NONE && pType == Vehicle.TYPE_NONE) - { - m_type = pType; - Destroy(); - } - } - - internal void Disable() - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - - if (m_aMotor != IntPtr.Zero) - { - - } - - } - - internal void Enable(IntPtr pBody, OdeScene pParentScene) - { - if (m_type == Vehicle.TYPE_NONE) - return; - - m_body = pBody; - // m_parentScene = pParentScene; - if (m_jointGroup == IntPtr.Zero) - m_jointGroup = d.JointGroupCreate(3); - - if (pBody != IntPtr.Zero) - { - - if (m_lMotor1 == IntPtr.Zero) - { - d.BodySetAutoDisableFlag(Body, false); - m_lMotor1 = d.JointCreateLMotor(pParentScene.world, m_jointGroup); - d.JointSetLMotorNumAxes(m_lMotor1, 1); - d.JointAttach(m_lMotor1, Body, IntPtr.Zero); - } - - if (m_aMotor == IntPtr.Zero) - { - m_aMotor = d.JointCreateAMotor(pParentScene.world, m_jointGroup); - d.JointSetAMotorNumAxes(m_aMotor, 3); - d.JointAttach(m_aMotor, Body, IntPtr.Zero); - } - } - } - - internal void Reset() - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - - } - - internal void Destroy() - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - if (m_aMotor != IntPtr.Zero) - { - d.JointDestroy(m_aMotor); - } - if (m_lMotor1 != IntPtr.Zero) - { - d.JointDestroy(m_lMotor1); - } - - } - - internal void Step(float pTimestep) - { - if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) - return; - frcount++; - if (frcount > 100) - frcount = 0; - - VerticalAttractor(pTimestep); - LinearMotor(pTimestep); - - - AngularMotor(pTimestep); - - } - - private void SetDefaultsForType(Vehicle pType) - { +Console.WriteLine("ProcessTypeChange to " + pType); + + // Set Defaults For Type m_type = pType; switch (pType) { @@ -342,10 +282,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularMotorDirection = Vector3.Zero; m_angularMotorTimescale = 1000; m_angularMotorDecayTimescale = 120; - // m_hoverHeight = 0; - // m_hoverEfficiency = 10; - // m_hoverTimescale = 10; - // m_buoyancy = 0; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 1; // m_linearDeflectionTimescale = 1; // m_angularDeflectionEfficiency = 1; @@ -368,10 +308,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularMotorDirection = Vector3.Zero; m_angularMotorTimescale = 1; m_angularMotorDecayTimescale = 0.8f; - // m_hoverHeight = 0; - // // m_hoverEfficiency = 0; - // // m_hoverTimescale = 1000; - // // m_buoyancy = 0; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; // // m_linearDeflectionEfficiency = 1; // // m_linearDeflectionTimescale = 2; // // m_angularDeflectionEfficiency = 0; @@ -395,10 +335,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularMotorDirection = Vector3.Zero; m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; - // m_hoverHeight = 0; - // m_hoverEfficiency = 0.5f; - // m_hoverTimescale = 2; - // m_buoyancy = 1; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0.5f; // m_linearDeflectionTimescale = 3; // m_angularDeflectionEfficiency = 0.5f; @@ -409,8 +349,9 @@ namespace OpenSim.Region.Physics.OdePlugin // m_bankingMix = 0.8f; // m_bankingTimescale = 1; // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_UP_ONLY | + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.LIMIT_MOTOR_UP); break; case Vehicle.TYPE_AIRPLANE: @@ -422,10 +363,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularMotorDirection = Vector3.Zero; m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; - // m_hoverHeight = 0; - // m_hoverEfficiency = 0.5f; - // m_hoverTimescale = 1000; - // m_buoyancy = 0; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 0.5f; // m_linearDeflectionTimescale = 3; // m_angularDeflectionEfficiency = 1; @@ -449,10 +390,10 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularMotorDirection = Vector3.Zero; m_angularMotorTimescale = 6; m_angularMotorDecayTimescale = 10; - // m_hoverHeight = 5; - // m_hoverEfficiency = 0.8f; - // m_hoverTimescale = 10; - // m_buoyancy = 1; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0; // m_linearDeflectionTimescale = 5; // m_angularDeflectionEfficiency = 0; @@ -463,106 +404,165 @@ namespace OpenSim.Region.Physics.OdePlugin // m_bankingMix = 0.7f; // m_bankingTimescale = 5; // m_referenceFrame = Quaternion.Identity; - m_flags = (VehicleFlag)0; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); break; } - } - - private void VerticalAttractor(float pTimestep) + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) { - // The purpose of this routine here is to quickly stabilize the Body while it's popped up in the air. - // The amotor needs a few seconds to stabilize so without it, the avatar shoots up sky high when you - // change appearance and when you enter the simulator - // After this routine is done, the amotor stabilizes much quicker - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - //d.BodyGetS +//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); + if (m_type == Vehicle.TYPE_NONE) + return; - d.Vector3 feet; - d.Vector3 head; - d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, -1.0f, out feet); - d.BodyGetRelPointPos(m_body, 0.0f, 0.0f, 1.0f, out head); - float posture = head.Z - feet.Z; - - //Console.WriteLine(String.Format("head: <{0},{1},{2}>, feet:<{3},{4},{5}> diff:<{6},{7},{8}>", head.X, head.Y, head.Z, feet.X, - // feet.Y, feet.Z, head.X - feet.X, head.Y - feet.Y, head.Z - feet.Z)); - //Console.WriteLine(String.Format("diff:<{0},{1},{2}>",head.X - feet.X, head.Y - feet.Y, head.Z - feet.Z)); - - // restoring force proportional to lack of posture: - float servo = (2.5f - posture) * (objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep)) * objMass.mass; - d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); - d.BodyAddForceAtRelPos(m_body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); - //d.BodyAddTorque(m_body, (head.X - feet.X) * servo, (head.Y - feet.Y) * servo, (head.Z - feet.Z) * servo); - //d.Matrix3 bodyrotation = d.BodyGetRotation(Body); - //m_log.Info("[PHYSICSAV]: Rotation: " + bodyrotation.M00 + " : " + bodyrotation.M01 + " : " + bodyrotation.M02 + " : " + bodyrotation.M10 + " : " + bodyrotation.M11 + " : " + bodyrotation.M12 + " : " + bodyrotation.M20 + " : " + bodyrotation.M21 + " : " + bodyrotation.M22); + m_body = pBody; + //KF: This used to set up the linear and angular joints } - private void LinearMotor(float pTimestep) + internal void Step(float pTimestep, OdeScene pParentScene) { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; - if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant { - - Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); - m_lastLinearVelocityVector += (addAmount*10); + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; - //Console.WriteLine("add: " + addAmount); - + + // decay applied velocity Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); //Console.WriteLine("decay: " + decayfraction); - m_linearMotorDirection -= m_linearMotorDirection * decayfraction; //Console.WriteLine("actual: " + m_linearMotorDirection); } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector - //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; +//if(frcount == 0) Console.WriteLine("herr0=" + herr0); + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + // m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); +//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); +//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); - SetLinearMotorProperties(); + // apply friction Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; - - //m_linearMotorDirection *= decayamount; - - } - - private void SetLinearMotorProperties() - { - Vector3 dirNorm = m_lastLinearVelocityVector; - dirNorm.Normalize(); - - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - dirNorm *= rotq; - if (m_lMotor1 != IntPtr.Zero) - { - - d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z); - d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length()); - - d.JointSetLMotorParam(m_lMotor1, (int)dParam.FMax, 35f * objMass.mass); - } - - } - - private void AngularMotor(float pTimestep) + } // end MoveLinear() + + private void MoveAngular(float pTimestep) { + + // m_angularMotorDirection is the latest value from the script, and is decayed here + // m_angularMotorDirectionLASTSET is the latest value from the script + // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here + if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) { - + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + // ramp up to new value Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); - m_lastAngularVelocityVector += (addAmount * 10); + m_lastAngularVelocityVector += (addAmount * 10f); +//if(frcount == 0) Console.WriteLine("add: " + addAmount); + // limit applied value to what was set by script // This will work temporarily, but we really need to compare speed on an axis if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; @@ -570,57 +570,61 @@ namespace OpenSim.Region.Physics.OdePlugin m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; - //Console.WriteLine("add: " + addAmount); + // decay the requested value Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); //Console.WriteLine("decay: " + decayfraction); - m_angularMotorDirection -= m_angularMotorDirection * decayfraction; //Console.WriteLine("actual: " + m_linearMotorDirection); } - - //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); - - SetAngularMotorProperties(); - + // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? + + // Vertical attractor section + +// d.Mass objMass; +// d.BodyGetMass(Body, out objMass); +// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by servo + verterr = verterr * servo; + + // rotate to object frame + // verterr = verterr * rotq; + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + m_lastAngularVelocityVector.X += verterr.Y; + m_lastAngularVelocityVector.Y -= verterr.X; +/* +if(frcount == 0) + { +// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); + Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", + Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); + } + */ + d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); + // apply friction Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; - //m_linearMotorDirection *= decayamount; - - } - private void SetAngularMotorProperties() - { - - - - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - //d.Quaternion rot = d.BodyGetQuaternion(Body); - //Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Vector3 axis0 = Vector3.UnitX; - Vector3 axis1 = Vector3.UnitY; - Vector3 axis2 = Vector3.UnitZ; - //axis0 *= rotq; - //axis1 *= rotq; - //axis2 *= rotq; - - - - if (m_aMotor != IntPtr.Zero) - { - d.JointSetAMotorAxis(m_aMotor, 0, 1, axis0.X, axis0.Y, axis0.Z); - d.JointSetAMotorAxis(m_aMotor, 1, 1, axis1.X, axis1.Y, axis1.Z); - d.JointSetAMotorAxis(m_aMotor, 2, 1, axis2.X, axis2.Y, axis2.Z); - d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax, 30*objMass.mass); - d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax2, 30*objMass.mass); - d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax3, 30 * objMass.mass); - d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel, m_lastAngularVelocityVector.X); - d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel2, m_lastAngularVelocityVector.Y); - d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel3, m_lastAngularVelocityVector.Z); - - } - } - + } //end MoveAngular } } diff --git a/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs new file mode 100644 index 0000000000..467eba081a --- /dev/null +++ b/OpenSim/Region/Physics/OdePlugin/ODEDynamics.cs @@ -0,0 +1,658 @@ +/* + * Revised Aug, Sept 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. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + public IntPtr Body + { + get { return m_body; } + } + + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private OdeScene m_parentScene = null; + private IntPtr m_body = IntPtr.Zero; + private IntPtr m_jointGroup = IntPtr.Zero; + private IntPtr m_aMotor = IntPtr.Zero; + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. + + + + + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; + if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; + if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + } + + }//end ProcessRotationVehicleParam + + internal void ProcessTypeChange(Vehicle pType) + { + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 100f; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) + { + if (m_type == Vehicle.TYPE_NONE) + return; + + m_body = pBody; + } + + internal void Step(float pTimestep, OdeScene pParentScene) + { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + */ + + // Get what the body is doing, this includes 'external' influences + d.Vector3 angularVelocity = d.BodyGetAngularVel(Body); + // Vector3 angularVelocity = Vector3.Zero; + + if (m_angularMotorApply > 0) + { + // ramp up to new value + // current velocity += error / ( time to get there / step interval ) + // requested speed - last motor speed + m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); + + m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected + // velocity may still be acheived. + } + else + { + // no motor recently applied, keep the body velocity + /* m_angularMotorVelocity.X = angularVelocity.X; + m_angularMotorVelocity.Y = angularVelocity.Y; + m_angularMotorVelocity.Z = angularVelocity.Z; */ + + // and decay the velocity + m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); + } // end motor section + + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if(m_verticalAttractionTimescale < 300) + { + float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by VAservo + verterr = verterr * VAservo; +//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; + + // scaling appears better usingsquare-law + float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); + vertattr.X += bounce * angularVelocity.X; + vertattr.Y += bounce * angularVelocity.Y; + + } // else vertical attractor is off + + // m_lastVertAttractor = vertattr; + + // Bank section tba + // Deflection section tba + + // Sum velocities + m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection + + if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + } + else + { + m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. + } + + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; + + // Apply to the body + d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + + } //end MoveAngular + } +} diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 673ae39ff6..34844c075d 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 (!_position.IsIdentical(m_taintposition,0f)) + changemove(timestep); - if (m_taintrot != _orientation) - rotate(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 (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { - 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; + 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(); + float m_mass = CalculateMass(); - fz = 0f; +// 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); + + //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; - //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 - 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; - if (m_usePID) - { - - //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() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - // no lock; for now it's only called from within Simulate() + 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; - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position + // PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } + 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 ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; + // if velocity is zero, use position control; otherwise, velocity control - // PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); + 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.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) - ); + //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; - // if velocity is zero, use position control; otherwise, velocity control + // 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; - if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) - // 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 + // 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; - //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; + // no lock; for now it's only called from within Simulate() - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } - 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; - } + 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; - } + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); - _target_velocity = - new PhysicsVector(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); + // 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; - // if velocity is zero, use position control; otherwise, velocity control + } // end switch (m_PIDHoverType) - 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 + _target_velocity = + new PhysicsVector(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); - 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; + // if velocity is zero, use position control; otherwise, velocity control - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } + 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 *= m_mass; - fy *= m_mass; - //fz *= m_mass; + 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; - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } - //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(); - } + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * 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 (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 diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs index 94223d8212..b7afa274be 100644 --- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs @@ -238,7 +238,8 @@ namespace OpenSim.Region.Physics.OdePlugin private readonly HashSet _characters = new HashSet(); private readonly HashSet _prims = new HashSet(); private readonly HashSet _activeprims = new HashSet(); - private readonly HashSet _taintedPrim = new HashSet(); + private readonly HashSet _taintedPrimH = new HashSet(); + private readonly List _taintedPrimL = new List(); private readonly HashSet _taintedActors = new HashSet(); private readonly List _perloopContact = new List(); private readonly List _collisionEventPrim = new List(); @@ -2112,6 +2113,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void RemovePrimThreadLocked(OdePrim prim) { +//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); lock (prim) { remCollisionEventReporting(prim); @@ -2559,11 +2561,15 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim is OdePrim) { OdePrim taintedprim = ((OdePrim) prim); - lock (_taintedPrim) + lock (_taintedPrimH) { - if (!(_taintedPrim.Contains(taintedprim))) - _taintedPrim.Add(taintedprim); - } + if (!(_taintedPrimH.Contains(taintedprim))) + { +//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName); + _taintedPrimH.Add(taintedprim); // HashSet for searching + _taintedPrimL.Add(taintedprim); // List for ordered readout + } + } return; } else if (prim is OdeCharacter) @@ -2599,7 +2605,7 @@ namespace OpenSim.Region.Physics.OdePlugin float fps = 0; //m_log.Info(timeStep.ToString()); step_time += timeStep; - + // If We're loaded down by something else, // or debugging with the Visual Studio project on pause // skip a few frames to catch up gracefully. @@ -2679,16 +2685,20 @@ namespace OpenSim.Region.Physics.OdePlugin // Modify other objects in the scene. processedtaints = false; - lock (_taintedPrim) + lock (_taintedPrimL) { - foreach (OdePrim prim in _taintedPrim) + foreach (OdePrim prim in _taintedPrimL) { + + if (prim.m_taintremove) { +//Console.WriteLine("Simulate calls RemovePrimThreadLocked"); RemovePrimThreadLocked(prim); } else { +//Console.WriteLine("Simulate calls ProcessTaints"); prim.ProcessTaints(timeStep); } processedtaints = true; @@ -2878,7 +2888,9 @@ namespace OpenSim.Region.Physics.OdePlugin } if (processedtaints) - _taintedPrim.Clear(); +//Console.WriteLine("Simulate calls Clear of _taintedPrim list"); + _taintedPrimH.Clear(); + _taintedPrimL.Clear(); } // Move characters @@ -3488,7 +3500,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (geom == localGround) { - //localHeightfield = TerrainHeightFieldHeights[geom]; + localHeightfield = TerrainHeightFieldHeights[geom]; proceed = true; } else @@ -3510,7 +3522,7 @@ namespace OpenSim.Region.Physics.OdePlugin // memory corruption if (TerrainHeightFieldHeights.ContainsKey(g)) { - //float[] removingHeightField = TerrainHeightFieldHeights[g]; + float[] removingHeightField = TerrainHeightFieldHeights[g]; TerrainHeightFieldHeights.Remove(g); if (RegionTerrain.ContainsKey(g)) @@ -3519,17 +3531,27 @@ namespace OpenSim.Region.Physics.OdePlugin } d.GeomDestroy(g); - //removingHeightField = new float[0]; - } + removingHeightField = new float[0]; + + + + } + } + } else { m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + } - } + + } + } - } + } + + public override void SetWaterLevel(float baseheight) { diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs index 02be98305d..edcf11ce01 100644 --- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs +++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs @@ -96,17 +96,21 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api m_localID = localID; m_itemID = itemID; - m_ScriptDelayFactor = m_ScriptEngine.Config.GetFloat("ScriptDelayFactor", 1.0f); - m_ScriptDistanceFactor = m_ScriptEngine.Config.GetFloat("ScriptDistanceLimitFactor", 1.0f); - m_MinTimerInterval = m_ScriptEngine.Config.GetFloat("MinTimerInterval", 0.5f); - m_automaticLinkPermission = m_ScriptEngine.Config.GetBoolean("AutomaticLinkPermission", false); - m_scriptConsoleChannel = m_ScriptEngine.Config.GetInt("ScriptConsoleChannel", 0); - m_scriptConsoleChannelEnabled = (m_scriptConsoleChannel != 0); - m_notecardLineReadCharsMax = m_ScriptEngine.Config.GetInt("NotecardLineReadCharsMax", 255); + m_ScriptDelayFactor = + m_ScriptEngine.Config.GetFloat("ScriptDelayFactor", 1.0f); + m_ScriptDistanceFactor = + m_ScriptEngine.Config.GetFloat("ScriptDistanceLimitFactor", 1.0f); + m_MinTimerInterval = + m_ScriptEngine.Config.GetFloat("MinTimerInterval", 0.5f); + m_automaticLinkPermission = + m_ScriptEngine.Config.GetBoolean("AutomaticLinkPermission", false); + m_notecardLineReadCharsMax = + m_ScriptEngine.Config.GetInt("NotecardLineReadCharsMax", 255); if (m_notecardLineReadCharsMax > 65535) m_notecardLineReadCharsMax = 65535; - m_TransferModule = m_ScriptEngine.World.RequestModuleInterface(); + m_TransferModule = + m_ScriptEngine.World.RequestModuleInterface(); m_UrlModule = m_ScriptEngine.World.RequestModuleInterface(); if (m_UrlModule != null) { @@ -1969,7 +1973,9 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api { part.UpdateRotation(rot); // Update rotation does not move the object in the physics scene if it's a linkset. - part.ParentGroup.AbsolutePosition = part.ParentGroup.AbsolutePosition; + +//KF: Do NOT use this next line if using ODE physics engine. This need a switch based on .ini Phys Engine type +// part.ParentGroup.AbsolutePosition = part.ParentGroup.AbsolutePosition; } /// @@ -6747,15 +6753,12 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api // try to let this work as in SL... if (part.ParentID == 0) { - // special case: If we are root, rotate - // complete SOG to new rotation + // special case: If we are root, rotate complete SOG to new rotation SetRot(part, Rot2Quaternion(q)); } else { - // we are a child. The rotation values - // will be set to the one of root modified - // by rot, as in SL. Don't ask. + // we are a child. The rotation values will be set to the one of root modified by rot, as in SL. Don't ask. SceneObjectGroup group = part.ParentGroup; if (group != null) // a bit paranoid, maybe {