diff --git a/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs b/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs index 5ca2ce43a3..c5cec5918d 100644 --- a/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs +++ b/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs @@ -417,7 +417,7 @@ namespace OpenSim.Region.CoreModules.Avatar.Attachments so.AttachedAvatar = UUID.Zero; rootPart.SetParentLocalId(0); so.ClearPartAttachmentData(); - rootPart.ApplyPhysics(rootPart.GetEffectiveObjectFlags(), rootPart.VolumeDetectActive); + rootPart.ApplyPhysics(rootPart.GetEffectiveObjectFlags(), rootPart.VolumeDetectActive,false); so.HasGroupChanged = true; rootPart.Rezzed = DateTime.Now; rootPart.RemFlag(PrimFlags.TemporaryOnRez); diff --git a/OpenSim/Region/Framework/Scenes/SOPVehicle.cs b/OpenSim/Region/Framework/Scenes/SOPVehicle.cs new file mode 100644 index 0000000000..39786bd625 --- /dev/null +++ b/OpenSim/Region/Framework/Scenes/SOPVehicle.cs @@ -0,0 +1,487 @@ +/* + * 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 OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using System.Xml; +using OpenSim.Framework.Serialization; +using OpenSim.Framework.Serialization.External; +using OpenSim.Region.Framework.Scenes.Serialization; + +namespace OpenSim.Region.Framework.Scenes +{ + public class SOPVehicle + { + public VehicleData vd; + + public Vehicle Type + { + get { return vd.m_type; } + } + + public SOPVehicle() + { + vd = new VehicleData(); + ProcessTypeChange(Vehicle.TYPE_NONE); // is needed? + } + + public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + vd.m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + vd.m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + vd.m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + vd.m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + vd.m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.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: + if (pValue < timestep) pValue = timestep; + vd.m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + vd.m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_angularMotorDirection.Length(); + if (len > 12.566f) + vd.m_angularMotorDirection *= (12.566f / len); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + vd.m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorDirection.Length(); + if (len > 30.0f) + vd.m_linearMotorDirection *= (30.0f / len); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + vd.m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorOffset.Length(); + if (len > 100.0f) + vd.m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + public void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + vd.m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + vd.m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + len = vd.m_angularMotorDirection.Length(); + if (len > 12.566f) + vd.m_angularMotorDirection *= (12.566f / len); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + vd.m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + vd.m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorDirection.Length(); + if (len > 30.0f) + vd.m_linearMotorDirection *= (30.0f / len); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + vd.m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorOffset.Length(); + if (len > 100.0f) + vd.m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessVectorVehicleParam + + public void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + vd.m_referenceFrame = Quaternion.Inverse(pValue); + break; + } + }//end ProcessRotationVehicleParam + + public void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + vd.m_flags &= ~((VehicleFlag)pParam); + } + else + { + vd.m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + public void ProcessTypeChange(Vehicle pType) + { + vd.m_linearMotorDirection = Vector3.Zero; + vd.m_angularMotorDirection = Vector3.Zero; + vd.m_linearMotorOffset = Vector3.Zero; + vd.m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + vd.m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + vd.m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 1000; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 1000; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1000; + vd.m_verticalAttractionEfficiency = 0; + vd.m_verticalAttractionTimescale = 1000; + + vd.m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + vd.m_linearFrictionTimescale = new Vector3(30, 1, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 120; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 1; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 10; + vd.m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + vd.m_linearFrictionTimescale = new Vector3(100, 2, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 1; + vd.m_angularMotorDecayTimescale = 0.8f; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 2; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 10; + vd.m_verticalAttractionEfficiency = 1f; + vd.m_verticalAttractionTimescale = 10f; + vd.m_bankingEfficiency = -0.2f; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + vd.m_linearFrictionTimescale = new Vector3(10, 3, 2); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 4; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 2; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 3; + vd.m_angularDeflectionEfficiency = 0.5f; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0.5f; + vd.m_verticalAttractionTimescale = 5f; + vd.m_bankingEfficiency = -0.3f; + vd.m_bankingMix = 0.8f; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_ROLL_ONLY); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + vd.m_linearFrictionTimescale = new Vector3(200, 10, 5); + vd.m_angularFrictionTimescale = new Vector3(20, 20, 20); + vd.m_linearMotorTimescale = 2; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 8; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 0.5f; + vd.m_angularDeflectionEfficiency = 1; + vd.m_angularDeflectionTimescale = 2; + vd.m_verticalAttractionEfficiency = 0.9f; + vd.m_verticalAttractionTimescale = 2f; + vd.m_bankingEfficiency = 1; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 2; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + vd.m_linearFrictionTimescale = new Vector3(5, 5, 5); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 6; + vd.m_angularMotorDecayTimescale = 10; + vd.m_VhoverHeight = 5; + vd.m_VhoverEfficiency = 0.8f; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 5; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0f; + vd.m_verticalAttractionTimescale = 1000f; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 5; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + } + } + public void SetVehicle(PhysicsActor ph) + { + if (ph == null) + return; + ph.SetVehicle(vd); + } + + private XmlTextWriter writer; + + private void XWint(string name, int i) + { + writer.WriteElementString(name, i.ToString()); + } + + private void XWfloat(string name, float f) + { + writer.WriteElementString(name, f.ToString(Utils.EnUsCulture)); + } + + private void XWVector(string name, Vector3 vec) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", vec.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", vec.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", vec.Z.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + private void XWQuat(string name, Quaternion quat) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", quat.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", quat.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", quat.Z.ToString(Utils.EnUsCulture)); + writer.WriteElementString("W", quat.W.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + public void ToXml2(XmlTextWriter twriter) + { + writer = twriter; + writer.WriteStartElement("SOGVehicle"); + + XWint("TYPE", (int)vd.m_type); + XWint("FLAGS", (int)vd.m_flags); + + // Linear properties + XWVector("LMDIR", vd.m_linearMotorDirection); + XWVector("LMFTIME", vd.m_linearFrictionTimescale); + XWfloat("LMDTIME", vd.m_linearMotorDecayTimescale); + XWfloat("LMTIME", vd.m_linearMotorTimescale); + XWVector("LMOFF", vd.m_linearMotorOffset); + + //Angular properties + XWVector("AMDIR", vd.m_angularMotorDirection); + XWfloat("AMTIME", vd.m_angularMotorTimescale); + XWfloat("AMDTIME", vd.m_angularMotorDecayTimescale); + XWVector("AMFTIME", vd.m_angularFrictionTimescale); + + //Deflection properties + XWfloat("ADEFF", vd.m_angularDeflectionEfficiency); + XWfloat("ADTIME", vd.m_angularDeflectionTimescale); + XWfloat("LDEFF", vd.m_linearDeflectionEfficiency); + XWfloat("LDTIME", vd.m_linearDeflectionTimescale); + + //Banking properties + XWfloat("BEFF", vd.m_bankingEfficiency); + XWfloat("BMIX", vd.m_bankingMix); + XWfloat("BTIME", vd.m_bankingTimescale); + + //Hover and Buoyancy properties + XWfloat("HHEI", vd.m_VhoverHeight); + XWfloat("HEFF", vd.m_VhoverEfficiency); + XWfloat("HTIME", vd.m_VhoverTimescale); + XWfloat("VBUO", vd.m_VehicleBuoyancy); + + //Attractor properties + XWfloat("VAEFF", vd.m_verticalAttractionEfficiency); + XWfloat("VATIME", vd.m_verticalAttractionTimescale); + + XWQuat("REF_FRAME", vd.m_referenceFrame); + + writer.WriteEndElement(); + writer = null; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs index 7b1ef4028f..6274e023b2 100644 --- a/OpenSim/Region/Framework/Scenes/Scene.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.cs @@ -4792,7 +4792,7 @@ Environment.Exit(1); bool wasUsingPhysics = ((jointProxyObject.Flags & PrimFlags.Physics) != 0); if (wasUsingPhysics) { - jointProxyObject.UpdatePrimFlags(false, false, true, false); // FIXME: possible deadlock here; check to make sure all the scene alterations set into motion here won't deadlock + jointProxyObject.UpdatePrimFlags(false, false, true, false,false); // FIXME: possible deadlock here; check to make sure all the scene alterations set into motion here won't deadlock } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index cbd45c3079..2481dd202a 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -43,6 +43,7 @@ using OpenSim.Region.Framework.Scenes.Serialization; namespace OpenSim.Region.Framework.Scenes { + [Flags] public enum scriptEvents { @@ -1378,7 +1379,8 @@ namespace OpenSim.Region.Framework.Scenes m_rootPart.SetParentLocalId(0); AttachmentPoint = (byte)0; - m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); + // must check if buildind should be true or false here + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive,false); HasGroupChanged = true; RootPart.Rezzed = DateTime.Now; RootPart.RemFlag(PrimFlags.TemporaryOnRez); @@ -1677,22 +1679,32 @@ namespace OpenSim.Region.Framework.Scenes /// public void ApplyPhysics() { - // Apply physics to the root prim - m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); - - // Apply physics to child prims SceneObjectPart[] parts = m_parts.GetArray(); if (parts.Length > 1) { + ResetChildPrimPhysicsPositions(); + + // Apply physics to the root prim + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, true); + + for (int i = 0; i < parts.Length; i++) { SceneObjectPart part = parts[i]; if (part.LocalId != m_rootPart.LocalId) - part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive); + part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive, true); } - // Hack to get the physics scene geometries in the right spot - ResetChildPrimPhysicsPositions(); +// ResetChildPrimPhysicsPositions(); + if (m_rootPart.PhysActor != null) + { + m_rootPart.PhysActor.Building = false; + } + } + else + { + // Apply physics to the root prim + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, false); } } @@ -1863,6 +1875,7 @@ namespace OpenSim.Region.Framework.Scenes /// public SceneObjectGroup Copy(bool userExposed) { + m_dupeInProgress = true; SceneObjectGroup dupe = (SceneObjectGroup)MemberwiseClone(); dupe.m_isBackedUp = false; dupe.m_parts = new MapAndArray(); @@ -1926,13 +1939,16 @@ namespace OpenSim.Region.Framework.Scenes pbs, newPart.AbsolutePosition, newPart.Scale, - newPart.RotationOffset, + //newPart.RotationOffset, + newPart.GetWorldRotation(), part.PhysActor.IsPhysical, newPart.LocalId); newPart.DoPhysicsPropertyUpdate(part.PhysActor.IsPhysical, true); } } + if (dupe.m_rootPart.PhysActor != null && userExposed) + dupe.m_rootPart.PhysActor.Building = false; // tell physics to finish building if (userExposed) { @@ -1943,6 +1959,7 @@ namespace OpenSim.Region.Framework.Scenes ScheduleGroupForFullUpdate(); } + m_dupeInProgress = false; return dupe; } @@ -2950,12 +2967,31 @@ namespace OpenSim.Region.Framework.Scenes } } +/* RootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); for (int i = 0; i < parts.Length; i++) { if (parts[i] != RootPart) parts[i].UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); } +*/ + if (parts.Length > 1) + { + m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, true); + + for (int i = 0; i < parts.Length; i++) + { + + if (parts[i].UUID != m_rootPart.UUID) + parts[i].UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, true); + } + + if (m_rootPart.PhysActor != null) + m_rootPart.PhysActor.Building = false; + } + else + m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, false); + } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index d6d04b2990..de1dd8caf1 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -310,6 +310,9 @@ namespace OpenSim.Region.Framework.Scenes private UUID m_collisionSound; private float m_collisionSoundVolume; + + private SOPVehicle m_vehicle = null; + #endregion Fields // ~SceneObjectPart() @@ -1506,7 +1509,8 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive) +// public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive) + public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive, bool building) { if (!ParentGroup.Scene.CollidablePrims) return; @@ -1542,7 +1546,8 @@ namespace OpenSim.Region.Framework.Scenes Shape, AbsolutePosition, Scale, - RotationOffset, +// RotationOffset, + GetWorldRotation(), // physics wants world rotation RigidBody, m_localId); } @@ -1557,8 +1562,16 @@ namespace OpenSim.Region.Framework.Scenes { PhysActor.SOPName = this.Name; // save object into the PhysActor so ODE internals know the joint/body info PhysActor.SetMaterial(Material); + + // if root part apply vehicle + if (m_vehicle != null && LocalId == ParentGroup.RootPart.LocalId) + m_vehicle.SetVehicle(PhysActor); + DoPhysicsPropertyUpdate(RigidBody, true); PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0); + + if (!building) + PhysActor.Building = false; } } } @@ -1794,6 +1807,10 @@ namespace OpenSim.Region.Framework.Scenes if (!isNew) ParentGroup.Scene.RemovePhysicalPrim(1); + Velocity = new Vector3(0, 0, 0); + Acceleration = new Vector3(0, 0, 0); + AngularVelocity = new Vector3(0, 0, 0); + PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate; PhysActor.OnOutOfBounds -= PhysicsOutOfBounds; PhysActor.delink(); @@ -3163,17 +3180,61 @@ namespace OpenSim.Region.Framework.Scenes } } + + public int VehicleType + { + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + SetVehicleType(value); + } + } + public void SetVehicleType(int type) { - if (PhysActor != null) + m_vehicle = null; + if (type == (int)Vehicle.TYPE_NONE) + { + if (_parentID ==0 && PhysActor != null) + PhysActor.VehicleType = (int)Vehicle.TYPE_NONE; + return; + } + m_vehicle = new SOPVehicle(); + m_vehicle.ProcessTypeChange((Vehicle)type); + { + if (_parentID ==0 && PhysActor != null) + PhysActor.VehicleType = type; + return; + } + } + + public void SetVehicleFlags(int param, bool remove) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessVehicleFlags(param, remove); + + if (_parentID ==0 && PhysActor != null) { - PhysActor.VehicleType = type; + PhysActor.VehicleFlags(param, remove); } } public void SetVehicleFloatParam(int param, float value) { - if (PhysActor != null) + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + + if (_parentID == 0 && PhysActor != null) { PhysActor.VehicleFloatParam(param, value); } @@ -3181,7 +3242,12 @@ namespace OpenSim.Region.Framework.Scenes public void SetVehicleVectorParam(int param, Vector3 value) { - if (PhysActor != null) + if (m_vehicle == null) + return; + + m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + + if (_parentID == 0 && PhysActor != null) { PhysActor.VehicleVectorParam(param, value); } @@ -3189,7 +3255,12 @@ namespace OpenSim.Region.Framework.Scenes public void SetVehicleRotationParam(int param, Quaternion rotation) { - if (PhysActor != null) + if (m_vehicle == null) + return; + + m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + + if (_parentID == 0 && PhysActor != null) { PhysActor.VehicleRotationParam(param, rotation); } @@ -3376,13 +3447,6 @@ namespace OpenSim.Region.Framework.Scenes hasProfileCut = hasDimple; // is it the same thing? } - public void SetVehicleFlags(int param, bool remove) - { - if (PhysActor != null) - { - PhysActor.VehicleFlags(param, remove); - } - } public void SetGroup(UUID groupID, IClientAPI client) { @@ -4270,7 +4334,8 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD) +// public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD) + public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD, bool building) { bool wasUsingPhysics = ((Flags & PrimFlags.Physics) != 0); bool wasTemporary = ((Flags & PrimFlags.TemporaryOnRez) != 0); @@ -4288,6 +4353,9 @@ namespace OpenSim.Region.Framework.Scenes // that... // ... if VD is changed, all others are not. // ... if one of the others is changed, VD is not. + // do this first + if (building && PhysActor != null && PhysActor.Building != building) + PhysActor.Building = building; if (SetVD) // VD is active, special logic applies { // State machine logic for VolumeDetect @@ -4369,11 +4437,17 @@ namespace OpenSim.Region.Framework.Scenes Shape, AbsolutePosition, Scale, - RotationOffset, +// RotationOffset, + GetWorldRotation(), //physics wants world rotation like all other functions send UsePhysics, m_localId); PhysActor.SetMaterial(Material); + + // if root part apply vehicle + if (m_vehicle != null && LocalId == ParentGroup.RootPart.LocalId) + m_vehicle.SetVehicle(PhysActor); + DoPhysicsPropertyUpdate(UsePhysics, true); if (!ParentGroup.IsDeleted) @@ -4449,6 +4523,9 @@ namespace OpenSim.Region.Framework.Scenes } // m_log.Debug("Update: PHY:" + UsePhysics.ToString() + ", T:" + IsTemporary.ToString() + ", PHA:" + IsPhantom.ToString() + " S:" + CastsShadows.ToString()); + // and last in case we have a new actor and not building + if (PhysActor != null && PhysActor.Building != building) + PhysActor.Building = building; if (ParentGroup != null) { ParentGroup.HasGroupChanged = true; diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs index 429fc068b8..8a426160d7 100644 --- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs +++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs @@ -1843,8 +1843,11 @@ namespace OpenSim.Region.Framework.Scenes // m_log.DebugFormat("[SCENE PRESENCE]: StandUp() for {0}", Name); SitGround = false; + +/* move this down so avatar gets physical in the new position and not where it is siting if (PhysicsActor == null) AddToPhysicalScene(false); + */ if (ParentID != 0) { @@ -1879,6 +1882,10 @@ namespace OpenSim.Region.Framework.Scenes ParentPosition = Vector3.Zero; ParentID = 0; + + if (PhysicsActor == null) + AddToPhysicalScene(false); + SendAvatarDataToAllAgents(); m_requestedSitTargetID = 0; @@ -1886,6 +1893,9 @@ namespace OpenSim.Region.Framework.Scenes part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK); } + else if (PhysicsActor == null) + AddToPhysicalScene(false); + Animator.TrySetMovementAnimation("STAND"); } diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index a68dcb7ce6..80c12770e5 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -22,11 +22,13 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Ubit 2012 * rolled into ODEPrim.cs */ using System; +using System.IO; using System.Collections.Generic; using System.Reflection; using System.Runtime.InteropServices; @@ -37,7 +39,6 @@ using Ode.NET; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; - namespace OpenSim.Region.Physics.OdePlugin { /// @@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + public class SerialControl + { + public object alock = new object(); + public byte[] data = new byte[0]; + } private Vector3 _position; private Vector3 _velocity; private Vector3 _torque; @@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin private float PID_D = 35f; private float PID_G = 25f; private bool m_usePID = false; - + private Quaternion m_APIDTarget = new Quaternion(); private float m_APIDStrength = 0.5f; private float m_APIDDamping = 0.5f; private bool m_useAPID = false; - private float m_APIDdamper = 1.0f; + private float m_APIDdamper = 1.0f; // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // do not confuse with VEHICLE HOVER @@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_taintPhysics; private bool m_collidesLand = true; private bool m_collidesWater; - public bool m_returnCollisions; + // public bool m_returnCollisions; // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); @@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene _parent_scene; public IntPtr m_targetSpace = IntPtr.Zero; public IntPtr prim_geom; -// public IntPtr prev_geom; + // public IntPtr prev_geom; public IntPtr _triMeshData; private IntPtr _linkJointGroup = IntPtr.Zero; @@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin private int throttleCounter; public int m_interpenetrationcount; public float m_collisionscore; - public int m_roundsUnderMotionThreshold; - private int m_crossingfailures; + // public int m_roundsUnderMotionThreshold; + // private int m_crossingfailures; public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; @@ -185,75 +191,525 @@ namespace OpenSim.Region.Physics.OdePlugin public volatile bool childPrim; internal int m_material = (int)Material.Wood; - - private int frcount = 0; // Used to limit dynamics debug output to - private int revcount = 0; // Reverse motion while > 0 private IntPtr m_body = 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; // Bit settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - + private VehicleFlag m_flags = (VehicleFlag)0; // Bit 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; // (was m_linearMotorDirectionLASTSET) the (local) Velocity - //requested by LSL - private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL - private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL + //requested by LSL + private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL + private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL - - private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor - private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity - private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity - + + private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor + private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity + private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity + //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor -// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity + // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - //Deflection properties + //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_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; // 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 + // 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. - - + + SerialControl m_taintserial = null; + object m_taintvehicledata = null; + + public void DoSetVehicle() + { + VehicleData vd = (VehicleData)m_taintvehicledata; + + m_type = vd.m_type; + m_flags = vd.m_flags; + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + m_linearMotorTimescale = vd.m_linearMotorTimescale; +// m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + + //Deflection properties +// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; +// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; +// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; +// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + + //Banking properties +// m_bankingEfficiency = vd.m_bankingEfficiency; +// m_bankingMix = vd.m_bankingMix; +// m_bankingTimescale = vd.m_bankingTimescale; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; +// m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + + // Axis +// m_referenceFrame = vd.m_referenceFrame; + m_taintvehicledata = null; + } + public override void SetVehicle(object vdata) + { + m_taintvehicledata = vdata; + _parent_scene.AddPhysicsActorTaint(this); + } + + public override byte[] Serialize(bool PhysIsRunning) + { + SerialControl sc = new SerialControl(); + + lock (sc.alock) + { + if (PhysIsRunning) + { + m_taintserial = sc; + + if (!Monitor.Wait(sc.alock, 1000)) + { + m_log.Error("[chOde] prim data serialization timed out"); + m_taintserial = null; + return new byte[0]; + } + } + else + DoSerialize(sc); + } + + return sc.data; + } + + public void DoSerialize(SerialControl sc) + { + wstreamer st = new wstreamer(); + Vector3 vtmp; + + ushort version = 2; + if (!BitConverter.IsLittleEndian) + version |= 1; + st.Wushort(version); //version lower bit codes endian type for future use + + // compact booleans in a ushort + ushort flags = 0; + + if (m_isphysical) // this should be true for now + flags |= 1; + if (m_isSelected) + flags |= 2; + if (m_isVolumeDetect) + flags |= 4; + if (m_disabled) + flags |= 8; + if (m_collidesWater) + flags |= 16; + if (m_collidesLand) + flags |= 32; + if (m_usePID) + flags |= 64; + if (m_useAPID) + flags |= 128; + if (m_useHoverPID) + flags |= 256; + if (m_throttleUpdates) + flags |= 512; + + st.Wushort(flags); + + st.Wvector3(_size); + st.Wint(m_material); + st.Wfloat(m_density); + st.Wfloat(0); // future gravity mod V3 + st.Wfloat(0); // future friction V3 + st.Wfloat(0); // future bounce V3 + +// st.Wuint((uint)m_collisionCategories); +// st.Wuint((uint)m_collisionFlags); + + if (_parent == null) + { + st.Wvector3(_position); // ?? + st.Wquat(_orientation); + } + else // for childs save offsets + { + Quaternion to; + Quaternion ipo = Quaternion.Inverse(_parent.Orientation); + + if (m_isphysical && prim_geom != IntPtr.Zero) + { + d.Vector3 dvt; + d.GeomCopyPosition(prim_geom, out dvt); + + vtmp.X = dvt.X; + vtmp.Y = dvt.Y; + vtmp.Z = dvt.Z; + + d.Quaternion dqt; + d.GeomCopyQuaternion(prim_geom, out dqt); + + to.X = dqt.X; + to.Y = dqt.Y; + to.Z = dqt.Z; + to.W = dqt.W; // rotation in world + } + else + { + vtmp = _position; + to = _orientation; + } + + vtmp -= _parent.Position; // offset in world + vtmp *= ipo; // offset in local + st.Wvector3(vtmp); + + ipo *= to; // own rotation + st.Wquat(ipo); + } + + st.Wvector3(_velocity); + st.Wvector3(m_rotationalVelocity); + st.Wvector3(_acceleration); + st.Wvector3(m_rotateEnable); + + vtmp = Vector3.Zero; + for (int i = 0; i < m_forcelist.Count; i++) + { + + vtmp += (m_forcelist[i] * 100); + } + + st.Wvector3(vtmp); // force acc + + vtmp = Vector3.Zero; + for (int i = 0; i < m_angularforcelist.Count; i++) + { + vtmp += (m_angularforcelist[i] * 100); + } + + st.Wvector3(vtmp); // angular force acc + + st.Wvector3(m_PIDTarget); + st.Wfloat(m_PIDTau); + st.Wfloat(PID_D); + st.Wfloat(PID_G); + st.Wquat(m_APIDTarget); + st.Wfloat(m_APIDStrength); + st.Wfloat(m_APIDDamping); + st.Wfloat(m_APIDdamper); + + st.Wint((int)m_PIDHoverType); + st.Wfloat(m_PIDHoverHeight); + st.Wfloat(m_PIDHoverTau); + st.Wfloat(m_targetHoverHeight); + + st.Wfloat(m_groundHeight); + st.Wfloat(m_waterHeight); + + st.Wfloat(m_buoyancy); + + // this must be last since type none ends stream + if (m_type == Vehicle.TYPE_NONE) + st.Wint((int)Vehicle.TYPE_NONE); + else + { + st.Wint((int)m_type); + + st.Wquat(Quaternion.Identity); //m_referenceFrame + + st.Wint((int)m_flags); + + st.Wvector3(m_linearMotorDirection); + st.Wfloat( + (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared())); + + st.Wvector3(m_linearFrictionTimescale); + st.Wfloat(m_linearMotorDecayTimescale); + st.Wfloat(m_linearMotorTimescale); + st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset); + + st.Wvector3(m_angularMotorDirection); + st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared())); + + st.Wvector3(m_angularFrictionTimescale); + st.Wfloat(m_angularMotorDecayTimescale); + st.Wfloat(m_angularMotorTimescale); + + st.Wfloat(0); //m_linearDeflectionEfficiency); + st.Wfloat(1000); //m_linearDeflectionTimescale); + + st.Wfloat(0); //m_angularDeflectionEfficiency); + st.Wfloat(120); //m_angularDeflectionTimescale); + + st.Wfloat(0); // m_bankingEfficiency); + st.Wfloat(0); //m_bankingMix); + st.Wfloat(1000); //m_bankingTimescale); + + st.Wfloat(m_VhoverHeight); + st.Wfloat(0.5f); //m_VhoverEfficiency); + st.Wfloat(m_VhoverTimescale); + + st.Wfloat(m_VehicleBuoyancy); + + st.Wfloat(m_verticalAttractionEfficiency); + st.Wfloat(m_verticalAttractionTimescale); + } + sc.data = st.close(); + m_taintserial = null; + Monitor.PulseAll(sc.alock); + } + + public bool DeSerialize(byte[] data) + { + rstreamer st = new rstreamer(data); + + int version =st.Rushort(); //version + + // merge booleans in a ushort + ushort flags = st.Rushort(); + if ((flags & 1) != 0) + m_isphysical = true; + if ((flags & 2) != 0) + m_taintselected = true; + if ((flags & 4) != 0) + m_isVolumeDetect = true; + if ((flags & 8) != 0) + m_taintdisable = true; + if ((flags & 16) != 0) + m_taintCollidesWater = true; + if ((flags & 32) != 0) + m_collidesLand = true; + if ((flags & 64) != 0) + m_usePID = true; + if ((flags & 128) != 0) + m_useAPID = true; + if ((flags & 256) != 0) + m_useHoverPID = true; + if ((flags & 512) != 0) + m_throttleUpdates = true; + + _size = st.Rvector3(); + m_taintsize = _size; + + m_material= st.Rint(); + m_density = st.Rfloat(); + st.Rfloat(); // future gravity mod V3 + st.Rfloat(); // future friction V3 + st.Rfloat(); // future bounce V3 + +// m_collisionCategories = (CollisionCategories)st.Ruint(); +// m_collisionFlags = (CollisionCategories) st.Ruint(); + + if (m_taintparent == null) + { + st.Rvector3(); // ignore old position sop/sog as to tell the new one + m_taintrot = st.Rquat(); // + _orientation = m_taintrot; + } + else + { + m_taintrot = _parent.Orientation; + m_taintposition = st.Rvector3(); // ?? + _position = m_taintposition; + + m_taintposition *= m_taintrot; + m_taintposition += _parent.Position; + + m_taintrot *= st.Rquat(); // + _orientation = m_taintrot; + } + + m_taintVelocity = st.Rvector3(); + m_rotationalVelocity = st.Rvector3(); + + _acceleration = st.Rvector3(); + m_rotateEnableRequest = st.Rvector3(); + m_rotateEnableUpdate = true; + + Vector3 vtmp; + + vtmp = st.Rvector3(); // forces acc + m_forcelist.Add(vtmp); + m_taintforce = true; + + vtmp = st.Rvector3(); // angular forces acc + m_angularforcelist.Add(vtmp); + m_taintaddangularforce = true; + + m_PIDTarget = st.Rvector3(); + m_PIDTau = st.Rfloat(); + PID_D = st.Rfloat(); + PID_G = st.Rfloat(); + + m_APIDTarget = st.Rquat(); + m_APIDStrength = st.Rfloat(); + m_APIDDamping = st.Rfloat(); + m_APIDdamper = st.Rfloat(); + + m_PIDHoverType = (PIDHoverType) st.Rint(); + m_PIDHoverHeight = st.Rfloat(); + m_PIDHoverTau = st.Rfloat(); + m_targetHoverHeight = st.Rfloat(); + + m_groundHeight = st.Rfloat(); + m_waterHeight = st.Rfloat(); + + m_buoyancy = st.Rfloat(); + + + // this must be last since type none ends stream + + m_type = (Vehicle) st.Rint(); + + if (m_type != Vehicle.TYPE_NONE) + { + float ftmp; + + st.Rquat(); //m_referenceFrame + + m_flags = (VehicleFlag) st.Rint(); + + m_linearMotorDirection = st.Rvector3(); + + ftmp = st.Rfloat(); + m_lLinMotorDVel = m_linearMotorDirection * ftmp; + + m_linearFrictionTimescale = st.Rvector3(); + m_linearMotorDecayTimescale = st.Rfloat(); + m_linearMotorTimescale = st.Rfloat(); + st.Rvector3(); //m_linearMotorOffset); + + m_angularMotorDirection = st.Rvector3(); + ftmp = st.Rfloat(); + m_angularMotorDVel = m_angularMotorDirection * ftmp; + + m_angularFrictionTimescale = st.Rvector3(); + m_angularMotorDecayTimescale = st.Rfloat(); + m_angularMotorTimescale = st.Rfloat(); + + st.Rfloat(); //m_linearDeflectionEfficiency); + st.Rfloat(); //m_linearDeflectionTimescale); + + st.Rfloat(); //m_angularDeflectionEfficiency); + st.Rfloat(); //m_angularDeflectionTimescale); + + st.Rfloat(); // m_bankingEfficiency); + st.Rfloat(); //m_bankingMix); + st.Rfloat(); //m_bankingTimescale); + + m_VhoverHeight = st.Rfloat(); + st.Rfloat(); //m_VhoverEfficiency); + m_VhoverTimescale = st.Rfloat(); + + m_VehicleBuoyancy = st.Rfloat(); + + m_verticalAttractionEfficiency = st.Rfloat(); + m_verticalAttractionTimescale = st.Rfloat(); + } + st.close(); + return true; + } + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent, + PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata) + { + m_localID = localid; + ode = dode; + + if (parent == null) + { + m_taintparent = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); + } + + _position = pos; + m_taintposition = pos; + } + else + m_taintparent = parent; + + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + + _mesh = null; + m_meshfailed = false; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = (IntPtr)0; + + if(sdata != null && sdata.Length > 1) + DeSerialize(sdata); + + if (m_isphysical) + m_targetSpace = _parent_scene.space; + + m_primName = primName; + m_taintserial = null; + m_taintadd = true; + _parent_scene.AddPhysicsActorTaint(this); + // don't do .add() here; old geoms get recycled with the same hash + } public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) @@ -266,7 +722,7 @@ namespace OpenSim.Region.Physics.OdePlugin parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); } - + _position = pos; m_taintposition = pos; PID_D = parent_scene.bodyPIDD; @@ -275,9 +731,8 @@ namespace OpenSim.Region.Physics.OdePlugin // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - prim_geom = IntPtr.Zero; -// prev_geom = IntPtr.Zero; + // prev_geom = IntPtr.Zero; if (!pos.IsFinite()) { @@ -306,7 +761,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene = parent_scene; m_targetSpace = (IntPtr)0; -// if (pos.Z < 0) + // if (pos.Z < 0) if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) m_isphysical = false; else @@ -317,6 +772,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) m_targetSpace = _parent_scene.space; } + + m_taintserial = null; m_primName = primName; m_taintadd = true; _parent_scene.AddPhysicsActorTaint(this); @@ -325,7 +782,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override int PhysicsActorType { - get { return (int) ActorTypes.Prim; } + get { return (int)ActorTypes.Prim; } set { return; } } @@ -337,9 +794,11 @@ namespace OpenSim.Region.Physics.OdePlugin public override uint LocalID { - set { + set + { //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; } + m_localID = value; + } } public override bool Grabbed @@ -349,9 +808,10 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool Selected { - set { - -//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); + set + { + + //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); // 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 @@ -367,21 +827,21 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } - if(m_isSelected) disableBodySoft(); + if (m_isSelected) disableBodySoft(); } } public override bool IsPhysical { get { return m_isphysical; } - set - { - m_isphysical = value; - if (!m_isphysical) - { // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - if (m_type != Vehicle.TYPE_NONE) Halt(); - } + set + { + m_isphysical = value; + if (!m_isphysical) + { // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + if (m_type != Vehicle.TYPE_NONE) Halt(); + } } } @@ -430,7 +890,9 @@ namespace OpenSim.Region.Physics.OdePlugin { get { return _position; } - set { _position = value; + set + { + _position = value; //m_log.Info("[PHYSICS]: " + _position.ToString()); } } @@ -481,29 +943,29 @@ namespace OpenSim.Region.Physics.OdePlugin public override void VehicleFloatParam(int param, float value) { - ProcessFloatVehicleParam((Vehicle) param, value); + ProcessFloatVehicleParam((Vehicle)param, value); } public override void VehicleVectorParam(int param, Vector3 value) { - ProcessVectorVehicleParam((Vehicle) param, value); + ProcessVectorVehicleParam((Vehicle)param, value); } public override void VehicleRotationParam(int param, Quaternion rotation) { - ProcessRotationVehicleParam((Vehicle) param, rotation); + ProcessRotationVehicleParam((Vehicle)param, rotation); } public override void VehicleFlags(int param, bool remove) { ProcessVehicleFlags(param, remove); } - + public override void SetVolumeDetect(int param) { lock (_parent_scene.OdeLock) { - m_isVolumeDetect = (param!=0); + m_isVolumeDetect = (param != 0); } } @@ -536,9 +998,9 @@ namespace OpenSim.Region.Physics.OdePlugin return Vector3.Zero; Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; return returnVelocity; } set @@ -546,8 +1008,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { _velocity = value; - if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) - _acceleration = Vector3.Zero; + if (_velocity.ApproxEquals(Vector3.Zero, 0.001f)) + _acceleration = Vector3.Zero; m_taintVelocity = value; _parent_scene.AddPhysicsActorTaint(this); @@ -604,17 +1066,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (QuaternionIsFinite(value)) { _orientation = value; - } + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); } } - - - public override bool FloatOnWater + + + public override bool FloatOnWater { - set { + set + { m_taintCollidesWater = value; _parent_scene.AddPhysicsActorTaint(this); } @@ -624,8 +1087,8 @@ namespace OpenSim.Region.Physics.OdePlugin { } - public override Vector3 PIDTarget - { + public override Vector3 PIDTarget + { set { if (value.IsFinite()) @@ -634,16 +1097,16 @@ namespace OpenSim.Region.Physics.OdePlugin } else m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); - } + } } public override bool PIDActive { set { m_usePID = value; } } public override float PIDTau { set { m_PIDTau = value; } } - - // For RotLookAt - public override Quaternion APIDTarget { set { m_APIDTarget = value; } } - public override bool APIDActive { set { m_useAPID = value; } } - public override float APIDStrength { set { m_APIDStrength = value; } } - public override float APIDDamping { set { m_APIDDamping = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } @@ -665,13 +1128,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Acceleration // client updates read data via here { - get - { + get + { if (_zeroFlag) { return Vector3.Zero; } - return _acceleration; + return _acceleration; } set { _acceleration = value; } } @@ -752,18 +1215,18 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); m_outofBounds = false; } -/* - int tmp = Interlocked.Increment(ref m_crossingfailures); - if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); - } - */ + /* + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + */ } public override float Buoyancy @@ -806,26 +1269,26 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetGeom(IntPtr geom) { - if(prim_geom != IntPtr.Zero) - { - // Remove any old entries -//string tPA; -//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); -//Console.WriteLine("**** Remove {0}", tPA); - if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); - if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); + if (prim_geom != IntPtr.Zero) + { + // Remove any old entries + //string tPA; + //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); + //Console.WriteLine("**** Remove {0}", tPA); + if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); + if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); d.GeomDestroy(prim_geom); - } + } prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); + //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { _parent_scene.geom_name_map[prim_geom] = this.m_primName; _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); -//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); + //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); } if (childPrim) @@ -833,7 +1296,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null && _parent is OdePrim) { OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); + //Console.WriteLine("SetGeom calls ChildSetGeom"); parent.ChildSetGeom(this); } } @@ -848,7 +1311,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyEnable(Body); if (m_type != Vehicle.TYPE_NONE) - Enable(Body, _parent_scene); + Enable(Body, _parent_scene); } m_disabled = false; @@ -892,9 +1355,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); + d.BodySetGravityMode(Body, false); m_interpenetrationcount = 0; m_collisionscore = 0; @@ -918,19 +1381,19 @@ namespace OpenSim.Region.Physics.OdePlugin float returnMass = 0; float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - + float hollowVolume = hollowAmount * hollowAmount; + switch (_pbs.ProfileShape) { case ProfileShape.Square: // default box if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; @@ -948,31 +1411,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { //a tube volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume*tmp*tmp; - + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; - + switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; case HollowShape.Circle: - hollowVolume *= 0.78539816339f;; + hollowVolume *= 0.78539816339f; ; break; case HollowShape.Triangle: @@ -981,23 +1444,23 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.Circle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.78539816339f; // elipse base if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1013,25 +1476,25 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); - + if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1047,31 +1510,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - volume *= 0.52359877559829887307710723054658f; + volume *= 0.52359877559829887307710723054658f; } break; case ProfileShape.EquilateralTriangle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.32475953f; if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1091,24 +1554,24 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.32475953f; volume *= 0.01f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1126,15 +1589,15 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } - break; + } + break; default: break; - } + } @@ -1148,7 +1611,7 @@ namespace OpenSim.Region.Physics.OdePlugin float profileEnd; if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { + { taperX1 = _pbs.PathScaleX * 0.01f; if (taperX1 > 1.0f) taperX1 = 2.0f - taperX1; @@ -1158,9 +1621,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (taperY1 > 1.0f) taperY1 = 2.0f - taperY1; taperY = 1.0f - taperY1; - } + } else - { + { taperX = _pbs.PathTaperX * 0.01f; if (taperX < 0.0f) taperX = -taperX; @@ -1168,10 +1631,10 @@ namespace OpenSim.Region.Physics.OdePlugin taperY = _pbs.PathTaperY * 0.01f; if (taperY < 0.0f) - taperY = -taperY; + taperY = -taperY; taperY1 = 1.0f - taperY; - } + } volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); @@ -1180,7 +1643,7 @@ namespace OpenSim.Region.Physics.OdePlugin pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; volume *= (pathEnd - pathBegin); -// this is crude aproximation + // this is crude aproximation profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; volume *= (profileEnd - profileBegin); @@ -1189,8 +1652,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. -// else if (returnMass > _parent_scene.maximumMassObject) -// returnMass = _parent_scene.maximumMassObject; + // else if (returnMass > _parent_scene.maximumMassObject) + // returnMass = _parent_scene.maximumMassObject; @@ -1230,7 +1693,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void setMass() { - if (Body != (IntPtr) 0) + if (Body != (IntPtr)0) { float newmass = CalculateMass(); @@ -1260,7 +1723,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + d.BodyDestroy(Body); lock (childrenPrim) { @@ -1279,7 +1742,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { _parent_scene.remActivePrim(this); - + m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); @@ -1289,7 +1752,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + Body = IntPtr.Zero; } } @@ -1345,10 +1808,10 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer - // { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - // } + // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer + // { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + // } } catch (AccessViolationException) { @@ -1357,14 +1820,14 @@ namespace OpenSim.Region.Physics.OdePlugin } - // if (IsPhysical && Body == (IntPtr) 0) - // { - // Recreate the body - // m_interpenetrationcount = 0; - // m_collisionscore = 0; + // if (IsPhysical && Body == (IntPtr) 0) + // { + // Recreate the body + // m_interpenetrationcount = 0; + // m_collisionscore = 0; - // enableBody(); - // } + // enableBody(); + // } } public void ProcessTaints(float timestep) //============================================================================= @@ -1373,37 +1836,37 @@ namespace OpenSim.Region.Physics.OdePlugin { changeadd(timestep); } - + if (prim_geom != IntPtr.Zero) { - if (!_position.ApproxEquals(m_taintposition, 0f)) + if (!_position.ApproxEquals(m_taintposition, 0f)) { - changemove(timestep); + changemove(timestep); } - if (m_taintrot != _orientation) - { - if(childPrim && IsPhysical) // For physical child prim... - { - rotate(timestep); - // KF: ODE will also rotate the parent prim! - // so rotate the root back to where it was - OdePrim parent = (OdePrim)_parent; - parent.rotate(timestep); + 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); - } + //Just rotate the prim + rotate(timestep); + } } // - + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) { changePhysicsStatus(timestep); }// - if (!_size.ApproxEquals(m_taintsize,0f)) + if (!_size.ApproxEquals(m_taintsize, 0f)) changesize(timestep); // @@ -1434,10 +1897,17 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); -/* obsolete - if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) - changeAngularLock(timestep); - */ + + if (m_taintvehicledata != null) + DoSetVehicle(); + + if (m_taintserial != null) + DoSerialize(m_taintserial); + + /* obsolete + if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) + changeAngularLock(timestep); + */ } else { @@ -1445,16 +1915,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } -/* obsolete - private void changeAngularLock(float timestep) - { - if (_parent == null) - { - m_angularLock = m_taintAngularLock; - m_angularLockSet = true; - } - } - */ + /* obsolete + private void changeAngularLock(float timestep) + { + if (_parent == null) + { + m_angularLock = m_taintAngularLock; + m_angularLockSet = true; + } + } + */ private void changelink(float timestep) { // If the newly set parent is not null @@ -1489,7 +1959,7 @@ namespace OpenSim.Region.Physics.OdePlugin childPrim = false; //_parent = null; } - + /* if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) d.JointGroupDestroy(_linkJointGroup); @@ -1498,7 +1968,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linkJoint = (IntPtr)0; */ } - + _parent = m_taintparent; m_taintPhysics = m_isphysical; } @@ -1512,8 +1982,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body == IntPtr.Zero) { Body = d.BodyCreate(_parent_scene.world); - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); setMass(); } @@ -1524,7 +1994,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childrenPrim.Contains(prim)) { childrenPrim.Add(prim); - + foreach (OdePrim prm in childrenPrim) { d.Mass m2; @@ -1546,7 +2016,7 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { - + prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); @@ -1571,7 +2041,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomSetBody(prm.prim_geom, Body); prm.childPrim = true; - d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); + d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z); //d.GeomSetOffsetPosition(prim.prim_geom, // (Position.X - prm.Position.X) - pMass.c.X, // (Position.Y - prm.Position.Y) - pMass.c.Y, @@ -1668,7 +2138,7 @@ namespace OpenSim.Region.Physics.OdePlugin ParentPrim(prm); } } - + } private void ChildDelink(OdePrim odePrim) @@ -1719,7 +2189,7 @@ namespace OpenSim.Region.Physics.OdePlugin // in between the disabling and the collision properties setting // which would wake the physical body up from a soft disabling and potentially cause it to fall // through the ground. - + // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select // just one part of the assembly, the rest of the assembly is non-selected and still simulating, // so that causes the selected part to wake up and continue moving. @@ -1752,12 +2222,12 @@ namespace OpenSim.Region.Physics.OdePlugin { disableBodySoft(); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } } @@ -1780,12 +2250,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } if (m_isphysical) @@ -1913,7 +2383,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_meshfailed = true; } // createmesh returns null when it's a shape that isn't a cube. - // m_log.Debug(m_localID); + // m_log.Debug(m_localID); } } @@ -1948,7 +2418,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical) { -// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! + // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { if (Body == IntPtr.Zero) @@ -1972,8 +2442,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("ODEPrim JointCreateFixed !!!"); + // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? + Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1991,8 +2461,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } //else - // { - //m_log.Debug("[BUG]: race!"); + // { + //m_log.Debug("[BUG]: race!"); //} } else @@ -2031,15 +2501,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); myrot.W = _orientation.W; if (Body != IntPtr.Zero) { - // KF: If this is a root prim do BodySet + // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - } - else - { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); } - + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + resetCollisionAccounting(); m_taintrot = _orientation; } @@ -2111,7 +2581,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void changesize(float timestamp) { - + string oldname = _parent_scene.geom_name_map[prim_geom]; if (_size.X <= 0) _size.X = 0.01f; @@ -2170,7 +2640,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); CreateGeom(m_targetSpace, mesh); - + } else { @@ -2210,7 +2680,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_taintsize = _size; } - + public void changefloatonwater(float timestep) { @@ -2398,7 +2868,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } d.BodyEnable(Body); d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); - + } m_angularforcelist.Clear(); } @@ -2420,7 +2890,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); } - + //resetCollisionAccounting(); } m_taintVelocity = Vector3.Zero; @@ -2428,9 +2898,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void UpdatePositionAndVelocity() { - return; // moved to the Move () method + return; // moved to the Move () method } - + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2455,7 +2925,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { _parent_scene.remCollisionEventReporting(this); m_eventsubscription = 0; - } + } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { @@ -2499,9 +2969,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public static Matrix4 Adjoint(Matrix4 pMat) { Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) + for (int i = 0; i < 4; i++) { - for (int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); } @@ -2524,7 +2994,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { if (j == iCol) continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); + Matrix4SetValue(ref minor, m, n, matrix[i, j]); n++; } m++; @@ -2622,18 +3092,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); private static float determinant3x3(Matrix4 pMat) { float det = 0; - float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; - float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; - float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; - float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; - float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; - float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; + float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2]; + float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0]; + float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1]; + float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2]; + float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0]; + float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1]; det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); return det; } - + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) { dst.c.W = src.c.W; @@ -2690,15 +3160,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; + 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_EFFICIENCY: + // if (pValue < 0f) pValue = 0f; + // if (pValue > 1f) pValue = 1f; + // m_VhoverEfficiency = pValue; + // break; case Vehicle.HOVER_HEIGHT: m_VhoverHeight = pValue; break; @@ -2731,12 +3201,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (pValue < 0.1f) pValue = 0.1f; 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: - if (pValue > 30f) pValue = 30f; - if (pValue < 0.1f) pValue = 0.1f; + if (pValue > 30f) pValue = 30f; + if (pValue < 0.1f) pValue = 0.1f; m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -2744,7 +3214,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < 0.1f) pValue = 0.1f; + if (pValue < 0.1f) pValue = 0.1f; m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2756,7 +3226,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; } - + }//end ProcessFloatVehicleParam internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) @@ -2764,29 +3234,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); switch (pParam) { case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X > 30f) pValue.X = 30f; - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y > 30f) pValue.Y = 30f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z > 30f) pValue.Z = 30f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X > 30f) pValue.X = 30f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y > 30f) pValue.Y = 30f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z > 30f) pValue.Z = 30f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; 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; + 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; UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2797,7 +3267,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); break; } - + }//end ProcessVectorVehicleParam internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) @@ -2808,31 +3278,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_referenceFrame = pValue; break; } - + }//end ProcessRotationVehicleParam - + internal void ProcessVehicleFlags(int pParam, bool remove) { if (remove) { - m_flags &= ~((VehicleFlag)pParam); - } - else - { - m_flags |= (VehicleFlag)pParam; - } + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } } - + internal void ProcessTypeChange(Vehicle pType) { - // Set Defaults For Type + // Set Defaults For Type m_type = pType; switch (pType) { case Vehicle.TYPE_SLED: m_linearFrictionTimescale = new Vector3(30, 1, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; m_angularMotorDirection = Vector3.Zero; @@ -2840,7 +3310,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1000; m_angularMotorDecayTimescale = 120; m_VhoverHeight = 0; -// m_VhoverEfficiency = 1; + // m_VhoverEfficiency = 1; m_VhoverTimescale = 10; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 1; @@ -2859,7 +3329,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2867,7 +3337,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1; m_angularMotorDecayTimescale = 0.8f; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0; + // m_VhoverEfficiency = 0; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // // m_linearDeflectionEfficiency = 1; @@ -2886,8 +3356,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; case Vehicle.TYPE_BOAT: m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); -// m_lLinMotorVel = Vector3.Zero; + m_angularFrictionTimescale = new Vector3(10, 10, 10); + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2895,7 +3365,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 2; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0.5f; @@ -2908,15 +3378,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // 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.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_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 2; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2924,7 +3394,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 0.5f; @@ -2951,7 +3421,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 6; m_angularMotorDecayTimescale = 10; m_VhoverHeight = 5; -// m_VhoverEfficiency = 0.8f; + // m_VhoverEfficiency = 0.8f; m_VhoverTimescale = 10; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0; @@ -2981,63 +3451,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } - internal void Halt() - { // Kill all motions, when non-physical - // m_linearMotorDirection = Vector3.Zero; - m_lLinMotorDVel = Vector3.Zero; - m_lLinObjectVel = Vector3.Zero; - m_wLinObjectVel = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; - m_lastAngularVelocity = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - _acceleration = Vector3.Zero; - } - - private void UpdateLinDecay() - { -// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; -// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; -// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - m_lLinMotorDVel.X = m_linearMotorDirection.X; - m_lLinMotorDVel.Y = m_linearMotorDirection.Y; - m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - } // else let the motor decay on its own + internal void Halt() + { // Kill all motions, when non-physical + // m_linearMotorDirection = Vector3.Zero; + m_lLinMotorDVel = Vector3.Zero; + m_lLinObjectVel = Vector3.Zero; + m_wLinObjectVel = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + _acceleration = Vector3.Zero; + } + + private void UpdateLinDecay() + { + m_lLinMotorDVel.X = m_linearMotorDirection.X; + m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + } // else let the motor decay on its own + + private void UpdateAngDecay() + { + m_angularMotorDVel.X = m_angularMotorDirection.X; + m_angularMotorDVel.Y = m_angularMotorDirection.Y; + m_angularMotorDVel.Z = m_angularMotorDirection.Z; + } // else let the motor decay on its own - private void UpdateAngDecay() - { -// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; -// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; -// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; - m_angularMotorDVel.X = m_angularMotorDirection.X; - m_angularMotorDVel.Y = m_angularMotorDirection.Y; - m_angularMotorDVel.Z = m_angularMotorDirection.Z; - } // else let the motor decay on its own - public void Move(float timestep) { float fx = 0; float fy = 0; float fz = 0; Vector3 linvel; // velocity applied, including any reversal - int outside = 0; - + // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. // This is a temp patch until proper region crossing is developed. - - int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; - float fence = _parent_scene.geomRegionFence; - - frcount++; // used to limit debug comment output - if (frcount > 50) - frcount = 0; - - if(revcount > 0) revcount--; - + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { - // Old public void UpdatePositionAndVelocity(), more accuratley calculated here - bool lastZeroFlag = _zeroFlag; // was it stopped - + // Old public void UpdatePositionAndVelocity(), more accuratley calculated here + bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); Vector3 l_position = Vector3.Zero; l_position.X = vec.X; @@ -3045,95 +3500,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); l_position.Z = vec.Z; m_lastposition = _position; _position = l_position; - + d.Quaternion ori = d.BodyGetQuaternion(Body); - // Quaternion l_orientation = Quaternion.Identity; + // Quaternion l_orientation = Quaternion.Identity; _orientation.X = ori.X; _orientation.Y = ori.Y; _orientation.Z = ori.Z; _orientation.W = ori.W; m_lastorientation = _orientation; - + d.Vector3 vel = d.BodyGetLinearVel(Body); m_lastVelocity = _velocity; _velocity.X = vel.X; _velocity.Y = vel.Y; _velocity.Z = vel.Z; _acceleration = ((_velocity - m_lastVelocity) / timestep); - + d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - - -//Console.WriteLine("Move {0} at {1}", m_primName, l_position); - /* - // Check if outside region - // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! - if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) - { - l_position.X = ((float)_parent_scene.WorldExtents.X - fence); - outside = 1; - } - - if (l_position.X < fence) - { - l_position.X = fence; - outside = 2; - } - if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) - { - l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); - outside = 3; - } - - if (l_position.Y < fence) - { - l_position.Y = fence; - outside = 4; - } - - if (outside > 0) - { - -//Console.WriteLine("Border {0} fence={1}", l_position, fence); - if (fence > 0.0f) // bounce object off boundary - { - if (revcount == 0) - { - if (outside < 3) - { - _velocity.X = -_velocity.X; - } - else - { - _velocity.Y = -_velocity.Y; - } - if (m_type != Vehicle.TYPE_NONE) Halt(); - _position = l_position; - m_taintposition = _position; - m_lastVelocity = _velocity; - _acceleration = Vector3.Zero; - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - base.RequestPhysicsterseUpdate(); - revcount = 25; // wait for object to move away from border - } - } // else old crossing mode - else if (m_crossingfailures < failureLimit) - { // keep trying to cross? - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) base.RequestPhysicsterseUpdate(); - return; // Dont process any other motion? - } - else - { // Too many tries - if (_parent == null) base.RaiseOutOfBounds(l_position); - return; // Dont process any other motion? - } // end various methods - } // end outside region horizontally - */ + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y ) @@ -3146,41 +3532,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - /* - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } - */ m_outofBounds = true; base.RequestPhysicsterseUpdate(); return; } -/* - if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) - { - // main simulator had a crossing failure - // park it inside region - _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(l_position.Z, -100f, 50000f); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - m_lastposition = _position; - - _velocity = Vector3.Zero; - m_lastVelocity = _velocity; - - - if (m_type != Vehicle.TYPE_NONE) - Halt(); - - d.BodySetLinearVel(Body, 0, 0, 0); - base.RequestPhysicsterseUpdate(); - return; - } -*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) @@ -3193,8 +3549,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); - - + + _acceleration.X = 0; // This stuff may stop client display but it has no _acceleration.Y = 0; // effect on the object in phys engine! _acceleration.Z = 0; @@ -3215,13 +3571,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //outofBounds = true; } // end neg Z check - // Is it moving? - /* 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) */ - if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly + // Is it moving? + /* 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) */ + if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating - (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly + (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly { _zeroFlag = true; m_throttleUpdates = false; @@ -3238,18 +3594,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { // Its stopped _velocity.X = 0.0f; _velocity.Y = 0.0f; - // _velocity.Z = 0.0f; + // _velocity.Z = 0.0f; _acceleration.X = 0; _acceleration.Y = 0; - // _acceleration.Z = 0; - + // _acceleration.Z = 0; + m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; // Stop it in the phys engine - d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); d.BodySetForce(Body, 0f, 0f, 0f); if (!m_lastUpdateSent) @@ -3287,71 +3643,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } m_lastposition = l_position; - - /// End UpdatePositionAndVelocity insert - - + + /// End UpdatePositionAndVelocity insert + + // Rotation lock ===================================== - if(m_rotateEnableUpdate) - { + if (m_rotateEnableUpdate) + { // Snapshot current angles, set up Amotor(s) m_rotateEnableUpdate = false; m_rotateEnable = m_rotateEnableRequest; -//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); + //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); if (Amotor != IntPtr.Zero) { d.JointDestroy(Amotor); Amotor = IntPtr.Zero; -//Console.WriteLine("Old Amotor Destroyed"); + //Console.WriteLine("Old Amotor Destroyed"); } - + if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) { // not all are enabled d.Quaternion r = d.BodyGetQuaternion(Body); Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); // extract the axes vectors - Vector3 vX = new Vector3(1f,0f,0f); - Vector3 vY = new Vector3(0f,1f,0f); - Vector3 vZ = new Vector3(0f,0f,1f); - vX = vX * locrot; + Vector3 vX = new Vector3(1f, 0f, 0f); + Vector3 vY = new Vector3(0f, 1f, 0f); + Vector3 vZ = new Vector3(0f, 0f, 1f); + vX = vX * locrot; vY = vY * locrot; vZ = vZ * locrot; // snapshot the current angle vectors m_lockX = vX; m_lockY = vY; m_lockZ = vZ; - // m_lockRot = locrot; + // m_lockRot = locrot; Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); d.JointAttach(Amotor, Body, IntPtr.Zero); d.JointSetAMotorMode(Amotor, 0); // User mode?? -//Console.WriteLine("New Amotor Created for {0}", m_primName); - + //Console.WriteLine("New Amotor Created for {0}", m_primName); + float axisnum = 3; // how many to lock axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); - d.JointSetAMotorNumAxes(Amotor,(int)axisnum); -//Console.WriteLine("AxisNum={0}",(int)axisnum); + d.JointSetAMotorNumAxes(Amotor, (int)axisnum); + //Console.WriteLine("AxisNum={0}",(int)axisnum); int i = 0; if (m_rotateEnable.X == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); -//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); + //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); i++; } if (m_rotateEnable.Y == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); -//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); + //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); i++; } if (m_rotateEnable.Z == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); -//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); + //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); i++; } @@ -3362,519 +3718,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); } // else none are locked } // end Rotation Update - - + + // VEHICLE processing ========================================== - if (m_type != Vehicle.TYPE_NONE) - { - // get body attitude - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion irotq = Quaternion.Inverse(rotq); - - // VEHICLE Linear Motion - d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame - Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); - m_lLinObjectVel = vel_now * irotq; - if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate - { - if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) - { - float decayfactor = m_linearMotorDecayTimescale/timestep; - Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); - m_lLinMotorDVel -= decayAmount; - } - else - { - float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); - Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; - m_lLinMotorDVel -= decel; - } - if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_lLinMotorDVel = Vector3.Zero; - } - - /* else - { - if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; - if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; - if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; - } */ - } // end linear motor decay - - if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - if (m_linearMotorTimescale < 300.0f) - { - Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; - float linfactor = m_linearMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/linfactor) * 1.3f; - m_lLinObjectVel += attackAmount; - } - if (m_linearFrictionTimescale.X < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.X / timestep; - float fricX = m_lLinObjectVel.X / fricfactor; - m_lLinObjectVel.X -= fricX; - } - if (m_linearFrictionTimescale.Y < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Y / timestep; - float fricY = m_lLinObjectVel.Y / fricfactor; - m_lLinObjectVel.Y -= fricY; - } - if (m_linearFrictionTimescale.Z < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Z / timestep; - float fricZ = m_lLinObjectVel.Z / fricfactor; - m_lLinObjectVel.Z -= fricZ; - } - } - m_wLinObjectVel = m_lLinObjectVel * rotq; - - // Gravity and Buoyancy - 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 = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force - } // else its 1.0, no gravity. - - // 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 = _parent_scene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _parent_scene.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 -// timestep 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_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); - //KF: m_VhoverEfficiency is not yet implemented - } - else - { - m_wLinObjectVel.Z = 0f; - } - } - else - { // not hovering - if (m_wLinObjectVel.Z == 0f) - { // Gravity rules - m_wLinObjectVel.Z = vel_now.Z; - } // else the motor has it - } - linvel = m_wLinObjectVel; + if (m_type != Vehicle.TYPE_NONE) + { + // get body attitude + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion irotq = Quaternion.Inverse(rotq); - // Vehicle Linear Motion done ======================================= - // Apply velocity - d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); - // end MoveLinear() + // VEHICLE Linear Motion + d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame + Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); + m_lLinObjectVel = vel_now * irotq; + if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate + { + if (Vector3.Mag(m_lLinMotorDVel) < 1.0f) + { + float decayfactor = m_linearMotorDecayTimescale / timestep; + Vector3 decayAmount = (m_lLinMotorDVel / decayfactor); + m_lLinMotorDVel -= decayAmount; + } + else + { + float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); + Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; + m_lLinMotorDVel -= decel; + } + if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lLinMotorDVel = Vector3.Zero; + } - - // MoveAngular - /* - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + /* else + { + if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; + if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } */ + } // end linear motor decay + + if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + if (m_linearMotorTimescale < 300.0f) + { + Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; + float linfactor = m_linearMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / linfactor) * 1.3f; + m_lLinObjectVel += attackAmount; + } + if (m_linearFrictionTimescale.X < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.X / timestep; + float fricX = m_lLinObjectVel.X / fricfactor; + m_lLinObjectVel.X -= fricX; + } + if (m_linearFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Y / timestep; + float fricY = m_lLinObjectVel.Y / fricfactor; + m_lLinObjectVel.Y -= fricY; + } + if (m_linearFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Z / timestep; + float fricZ = m_lLinObjectVel.Z / fricfactor; + m_lLinObjectVel.Z -= fricZ; + } + } + m_wLinObjectVel = m_lLinObjectVel * rotq; + + // Gravity and Buoyancy + 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 = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force + } // else its 1.0, no gravity. + + // 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 = _parent_scene.GetWaterLevel() + m_VhoverHeight; + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _parent_scene.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 + // timestep 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_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_wLinObjectVel.Z = 0f; + } + } + else + { // not hovering + if (m_wLinObjectVel.Z == 0f) + { // Gravity rules + m_wLinObjectVel.Z = vel_now.Z; + } // else the motor has it + } + linvel = m_wLinObjectVel; + + // Vehicle Linear Motion done ======================================= + // Apply velocity + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); + //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); + // end MoveLinear() + + + // MoveAngular + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL - private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL - private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor - private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body - */ -//if(frcount == 0) Console.WriteLine("MoveAngular "); - - d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); - Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); - angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation - -//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor + private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body + */ + //if(frcount == 0) Console.WriteLine("MoveAngular "); - // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. - float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); - m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; - // Decay Angular Motor 2. - if (m_angularMotorDecayTimescale < 300.0f) - { - if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) - { - float decayfactor = (m_angularMotorDecayTimescale)/timestep; - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); - m_angularMotorDVel -= decayAmount; - } - else - { - Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; - m_angularMotorDVel -= decel; - } - - if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_angularMotorDVel = Vector3.Zero; - } - else - { - if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; - if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; - if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; - } - } // end decay angular motor -//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); + d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); + Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); + angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation -//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); - if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { // if motor or object have motion - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - if (m_angularMotorTimescale < 300.0f) - { - Vector3 attack_error = m_angularMotorDVel - angObjectVel; - float angfactor = m_angularMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/angfactor); - angObjectVel += attackAmount; -//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); -//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); - } - - angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); - angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); - angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); - } // else no signif. motion - -//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); - // Bank section tba - // Deflection section tba -//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - - - /* // Rotation Axis Disables: - if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - if (m_angularEnable.X == 0) - angObjectVel.X = 0f; - if (m_angularEnable.Y == 0) - angObjectVel.Y = 0f; - if (m_angularEnable.Z == 0) - angObjectVel.Z = 0f; - } - */ - angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation - - // Vertical attractor section - Vector3 vertattr = Vector3.Zero; - - if(m_verticalAttractionTimescale < 300) - { - float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); - // 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) - { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to - // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. -//Console.WriteLine("InvertFlip"); - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - verterr *= 0.5f; - // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) - Vector3 xyav = angObjectVel; - xyav.Z = 0.0f; - if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) - { - // 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; -//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // scaling appears better usingsquare-law - float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; - float bounce = 1.0f - damped; - // 0 = crit damp, 1 = bouncy - float oavz = angObjectVel.Z; // retain z velocity - // time-scaled correction, which sums, therefore is bouncy: - angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; - // damped, good @ < 90: - angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); - angObjectVel.Z = oavz; -//if(frcount == 0) Console.WriteLine("VA+"); -//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); - } - else - { - // else error is very small - angObjectVel.X = 0f; - angObjectVel.Y = 0f; -//if(frcount == 0) Console.WriteLine("VA0"); - } - } // else vertical attractor is off -//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. + float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); + m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; + // Decay Angular Motor 2. + if (m_angularMotorDecayTimescale < 300.0f) + { + if (Vector3.Mag(m_angularMotorDVel) < 1.0f) + { + float decayfactor = (m_angularMotorDecayTimescale) / timestep; + Vector3 decayAmount = (m_angularMotorDVel / decayfactor); + m_angularMotorDVel -= decayAmount; + } + else + { + Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; + m_angularMotorDVel -= decel; + } - - m_lastAngularVelocity = angObjectVel; - // apply Angular Velocity to body - d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); -//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); - - } // end VEHICLES - else - { - // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ - - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - - /// Dynamics Buoyancy - //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // NB Prims in ODE are no subject to global gravity - // This should only affect gravity operations - - float m_mass = CalculateMass(); - // calculate z-force due togravity on object. - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. - { - fz = 0; // llMoveToTarget ignores gravity. - // it also ignores mass of object, and any physical resting on it. - // Vector3 m_PIDTarget is where we are going - // float m_PIDTau is time to get there + if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_angularMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; + if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; + if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; + } + } // end decay angular motor + //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); + + //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + + if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { // if motor or object have motion + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / angfactor); + angObjectVel += attackAmount; + //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); + //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); + } // else no signif. motion + + //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); + // Bank section tba + // Deflection section tba + //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); + + + /* // Rotation Axis Disables: + if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularEnable.X == 0) + angObjectVel.X = 0f; + if (m_angularEnable.Y == 0) + angObjectVel.Y = 0f; + if (m_angularEnable.Z == 0) + angObjectVel.Z = 0f; + } + */ + angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); + // 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) + { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to + // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. + //Console.WriteLine("InvertFlip"); + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + verterr *= 0.5f; + // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) + Vector3 xyav = angObjectVel; + xyav.Z = 0.0f; + if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + { + // 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; + //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // scaling appears better usingsquare-law + float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; + float bounce = 1.0f - damped; + // 0 = crit damp, 1 = bouncy + float oavz = angObjectVel.Z; // retain z velocity + // time-scaled correction, which sums, therefore is bouncy: + angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; + // damped, good @ < 90: + angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); + angObjectVel.Z = oavz; + //if(frcount == 0) Console.WriteLine("VA+"); + //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); + } + else + { + // else error is very small + angObjectVel.X = 0f; + angObjectVel.Y = 0f; + //if(frcount == 0) Console.WriteLine("VA0"); + } + } // else vertical attractor is off + //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + + + m_lastAngularVelocity = angObjectVel; + // apply Angular Velocity to body + d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); + + } // end VEHICLES + else + { + // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ + + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + /// Dynamics Buoyancy + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // NB Prims in ODE are no subject to global gravity + // This should only affect gravity operations + + float m_mass = CalculateMass(); + // calculate z-force due togravity on object. + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. + { + fz = 0; // llMoveToTarget ignores gravity. + // it also ignores mass of object, and any physical resting on it. + // Vector3 m_PIDTarget is where we are going + // float m_PIDTau is time to get there fx = 0; - fy = 0; - d.Vector3 pos = d.BodyGetPosition(Body); - Vector3 error = new Vector3( - (m_PIDTarget.X - pos.X), - (m_PIDTarget.Y - pos.Y), - (m_PIDTarget.Z - pos.Z)); - if (error.ApproxEquals(Vector3.Zero,0.01f)) - { // Very close, Jump there and quit move + fy = 0; + d.Vector3 pos = d.BodyGetPosition(Body); + Vector3 error = new Vector3( + (m_PIDTarget.X - pos.X), + (m_PIDTarget.Y - pos.Y), + (m_PIDTarget.Z - pos.Z)); + if (error.ApproxEquals(Vector3.Zero, 0.01f)) + { // Very close, Jump there and quit move - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - _target_velocity = Vector3.Zero; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + _target_velocity = Vector3.Zero; d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); d.BodySetForce(Body, 0f, 0f, 0f); - } - else - { - float scale = 50.0f * timestep / m_PIDTau; - if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) - { - // Nearby, quit update of velocity - } - else - { // Far, calc damped velocity - _target_velocity = error * scale; - } - d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); } - } // end PID MoveToTarget - - - /// Dynamics Hover =================================================================================== - // Hover PID Controller can only run if the PIDcontroller is not in use. - if (m_useHoverPID && !m_usePID) - { -//Console.WriteLine("Hover " + m_primName); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); -// d.Vector3 vel = d.BodyGetLinearVel(Body); + else + { + float scale = 50.0f * timestep / m_PIDTau; + if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero)) + { + // Nearby, quit update of velocity + } + else + { // Far, calc damped velocity + _target_velocity = error * scale; + } + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end PID MoveToTarget - // 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; + /// Dynamics Hover =================================================================================== + // Hover PID Controller can only run if the PIDcontroller is not in use. + if (m_useHoverPID && !m_usePID) + { + //Console.WriteLine("Hover " + m_primName); - } // end switch (m_PIDHoverType) + // 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; + } - _target_velocity = + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + // d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); - // if velocity is zero, use position control; otherwise, velocity control + // if velocity is zero, use position control; otherwise, velocity control - if (_target_velocity.ApproxEquals(Vector3.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 dlinvel = vel; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); - d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; - } - else - { - _zeroFlag = false; + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end m_useHoverPID && !m_usePID - - - /// Dynamics Apply Forces =================================================================================== - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; + // 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 dlinvel = vel; + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); + d.BodyAddForce(Body, 0, 0, fz); + //KF this prevents furthur motions return; + } + else + { + _zeroFlag = false; - //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, 0f, 0f, 0f); - enableBodySoft(); - } + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end m_useHoverPID && !m_usePID - // 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); - } // end apply forces - } // end Vehicle/Dynamics + /// Dynamics Apply Forces =================================================================================== + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; - /// RotLookAt / LookAt ================================================================================= + //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, 0f, 0f, 0f); + 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); + } // end apply forces + } // end Vehicle/Dynamics + + /// RotLookAt / LookAt ================================================================================= if (m_useAPID) { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: - // get present body rotation - float limit = 1.0f; - float rscaler = 50f; // adjusts rotation damping time - float lscaler = 10f; // adjusts linear damping time in llLookAt - float RLAservo = 0f; + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: + // get present body rotation + float limit = 1.0f; + float rscaler = 50f; // adjusts rotation damping time + float lscaler = 10f; // adjusts linear damping time in llLookAt + float RLAservo = 0f; Vector3 diff_axis; float diff_angle; - d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rtarget = new Quaternion(); + d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rtarget = new Quaternion(); - if(m_APIDTarget.W == -99.9f) - { - // this is really a llLookAt(), x,y,z is the target vector - Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); - Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; + if (m_APIDTarget.W == -99.9f) + { + // this is really a llLookAt(), x,y,z is the target vector + Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); + Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); float twopi = 2.0f * (float)Math.PI; - Vector3 dir = target - _position; - dir.Normalize(); + Vector3 dir = target - _position; + dir.Normalize(); float tzrot = (float)Math.Atan2(dir.Y, dir.X); float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); float terot = (float)Math.Atan2(dir.Z, txy); @@ -3882,63 +4238,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); float oerot = (float)Math.Atan2(ospin.Z, oxy); float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); - float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); + float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X)); float roll = (float)Math.Atan2(ra, rb); float errorz = tzrot - ozrot; - if(errorz > (float)Math.PI) errorz -= twopi; - else if(errorz < -(float)Math.PI) errorz += twopi; + if (errorz > (float)Math.PI) errorz -= twopi; + else if (errorz < -(float)Math.PI) errorz += twopi; float errory = oerot - terot; - if(errory > (float)Math.PI) errory -= twopi; - else if(errory < -(float)Math.PI) errory += twopi; + if (errory > (float)Math.PI) errory -= twopi; + else if (errory < -(float)Math.PI) errory += twopi; diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); - if(diff_angle > 0.01f * m_APIDdamper) - { - m_APIDdamper = 1.0f; - RLAservo = timestep / m_APIDStrength * rscaler; + if (diff_angle > 0.01f * m_APIDdamper) + { + m_APIDdamper = 1.0f; + RLAservo = timestep / m_APIDStrength * rscaler; errorz *= RLAservo; errory *= RLAservo; error.X = -roll * 8.0f; error.Y = errory; error.Z = errorz; error *= rotq; - d.BodySetAngularVel (Body, error.X, error.Y, error.Z); - } - else - { - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } - else - { - // this is a llRotLookAt() - rtarget = m_APIDTarget; + d.BodySetAngularVel(Body, error.X, error.Y, error.Z); + } + else + { + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } + else + { + // this is a llRotLookAt() + rtarget = m_APIDTarget; - Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle -//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); + Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle + //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); - // diff_axis.Normalize(); it already is! - if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error - { - m_APIDdamper = 1.0f; - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate - RLAservo = timestep / m_APIDStrength * lscaler; - rotforce = rotforce * RLAservo * diff_angle ; - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } - else - { // close enough - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } // end llLookAt/llRotLookAt -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + // diff_axis.Normalize(); it already is! + if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error + { + m_APIDdamper = 1.0f; + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if (diff_angle > limit) diff_angle = limit; // cap the rotate rate + RLAservo = timestep / m_APIDStrength * lscaler; + rotforce = rotforce * RLAservo * diff_angle; + d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z); + //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } + else + { // close enough + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } // end llLookAt/llRotLookAt + //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID } // end root prims - } // end Move() - } // end class + } // end Move() + } // end class } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index cf7c1d717d..b5b30ee33d 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } + private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent, + PrimitiveBaseShape pbs, uint localid, byte[] sdata) + { + Vector3 pos = position; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata); + lock (_prims) + _prims.Add(newPrim); + } + + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) { // adds active prim.. (ones that should be iterated over in collisions_optimized @@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } + public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + PhysicsActor result; + + result = AddPrim(primName, position, parent, + pbs, localid, sdata); + + return result; + } + public override float TimeDilation { get { return m_timeDilation; } @@ -3412,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapWidth = regionsize + 2; // ODE map size 258 x 258 (Meters) (1 extra each side) int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples - int heightmapHeightSamples = (int)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 3; // to have 258m we need 259 samples + int heightmapHeightSamples = (int)regionsize + 3; // Array of height samples for ODE float[] _heightmap; - _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 259 x 259 // Other ODE parameters const float scale = 1.0f; diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs new file mode 100644 index 0000000000..e7e7bb3623 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs @@ -0,0 +1,353 @@ +/* Ubit 2012 + * 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. +*/ + +// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian + +using System; +using System.IO; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + + unsafe public class wstreamer + { + byte[] buf; + int index; + byte* src; + + public wstreamer() + { + buf = new byte[1024]; + index = 0; + } + public wstreamer(int size) + { + buf = new byte[size]; + index = 0; + } + + public byte[] close() + { + byte[] data = new byte[index]; + Buffer.BlockCopy(buf, 0, data, 0, index); + return data; + } + + public void Seek(int pos) + { + index = pos; + } + + public void Seekrel(int pos) + { + index += pos; + } + + public void Wbyte(byte value) + { + buf[index++] = value; + } + public void Wshort(short value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wushort(ushort value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wint(int value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wuint(uint value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wlong(long value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wulong(ulong value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + + public void Wfloat(float value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + + public void Wdouble(double value) + { + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + + public void Wvector3(Vector3 value) + { + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + public void Wquat(Quaternion value) + { + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.W; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + } + } + + unsafe public class rstreamer + { + private byte[] rbuf; + private int ptr; + private byte* dst; + + public rstreamer(byte[] data) + { + rbuf = data; + ptr = 0; + } + + public void close() + { + } + + public void Seek(int pos) + { + ptr = pos; + } + + public void Seekrel(int pos) + { + ptr += pos; + } + + public byte Rbyte() + { + return (byte)rbuf[ptr++]; + } + + public short Rshort() + { + short v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public ushort Rushort() + { + ushort v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public int Rint() + { + int v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public uint Ruint() + { + uint v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public long Rlong() + { + long v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public ulong Rulong() + { + ulong v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + public float Rfloat() + { + float v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + + public double Rdouble() + { + double v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + + public Vector3 Rvector3() + { + Vector3 v; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; + } + + public Quaternion Rquat() + { + Quaternion v; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.W; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + return v; + } + } +} diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 3a9ca1bd34..cc9248450a 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -65,5 +65,6 @@ namespace OpenSim.Region.Physics.Manager void releasePinned(); void Append(IMesh newMesh); void TransformLinear(float[,] matrix, float[] offset); + Vector3 GetCentroid(); } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 0587054c23..f525e9e51e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -68,6 +68,17 @@ namespace OpenSim.Region.Physics.Manager } } + public struct ContactData + { + public float mu; + public float bounce; + + public ContactData(float _mu, float _bounce) + { + mu = _mu; + bounce = _bounce; + } + } /// /// Used to pass collision information to OnCollisionUpdate listeners. /// @@ -135,6 +146,8 @@ namespace OpenSim.Region.Physics.Manager /// public event CollisionUpdate OnCollisionUpdate; + public virtual void SetVehicle(object vdata) { } + public event OutOfBounds OnOutOfBounds; #pragma warning restore 67 @@ -142,6 +155,13 @@ namespace OpenSim.Region.Physics.Manager { get { return new NullPhysicsActor(); } } + + public virtual bool Building { get; set; } + + public virtual ContactData ContactData + { + get { return new ContactData(0, 0); } + } public abstract bool Stopped { get; } @@ -195,6 +215,11 @@ namespace OpenSim.Region.Physics.Manager } } + public virtual byte[] Serialize(bool PhysIsRunning) + { + return new byte[0]; + } + public virtual void RaiseOutOfBounds(Vector3 pos) { // Make a temporary copy of the event to avoid possibility of @@ -554,5 +579,6 @@ namespace OpenSim.Region.Physics.Manager { return false; } + } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 514d9ad3e2..0346d4e2f0 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -128,6 +128,12 @@ namespace OpenSim.Region.Physics.Manager public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid); + public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + return null; + } + public virtual float TimeDilation { get { return 1.0f; } @@ -225,7 +231,7 @@ namespace OpenSim.Region.Physics.Manager } public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} - + public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {} public virtual void UnCombine(PhysicsScene pScene) {} /// @@ -263,5 +269,13 @@ namespace OpenSim.Region.Physics.Manager { return new List(); } + + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } + public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + return new List(); + } + } } diff --git a/OpenSim/Region/Physics/Manager/VehicleConstants.cs b/OpenSim/Region/Physics/Manager/VehicleConstants.cs index f0775c1a8e..8e24b4c81e 100644 --- a/OpenSim/Region/Physics/Manager/VehicleConstants.cs +++ b/OpenSim/Region/Physics/Manager/VehicleConstants.cs @@ -26,6 +26,7 @@ */ using System; +using OpenMetaverse; namespace OpenSim.Region.Physics.Manager { @@ -117,5 +118,47 @@ namespace OpenSim.Region.Physics.Manager NO_DEFLECTION = 16392, LOCK_ROTATION = 32784 } - + + public struct VehicleData + { + public Vehicle m_type; + public VehicleFlag m_flags; + + // Linear properties + public Vector3 m_linearMotorDirection; + public Vector3 m_linearFrictionTimescale; + public float m_linearMotorDecayTimescale; + public float m_linearMotorTimescale; + public Vector3 m_linearMotorOffset; + + //Angular properties + public Vector3 m_angularMotorDirection; + public float m_angularMotorTimescale; + public float m_angularMotorDecayTimescale; + public Vector3 m_angularFrictionTimescale; + + //Deflection properties + public float m_angularDeflectionEfficiency; + public float m_angularDeflectionTimescale; + public float m_linearDeflectionEfficiency; + public float m_linearDeflectionTimescale; + + //Banking properties + public float m_bankingEfficiency; + public float m_bankingMix; + public float m_bankingTimescale; + + //Hover and Buoyancy properties + public float m_VhoverHeight; + public float m_VhoverEfficiency; + public float m_VhoverTimescale; + public float m_VehicleBuoyancy; + + //Attractor properties + public float m_verticalAttractionEfficiency; + public float m_verticalAttractionTimescale; + + // Axis + public Quaternion m_referenceFrame; + } } diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs index f781ff9bd0..c715642bac 100644 --- a/OpenSim/Region/Physics/Meshing/Mesh.cs +++ b/OpenSim/Region/Physics/Meshing/Mesh.cs @@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing IntPtr m_indicesPtr = IntPtr.Zero; int m_indexCount = 0; public float[] m_normals; + Vector3 _centroid; + int _centroidDiv; + + private class vertexcomp : IEqualityComparer + { + public bool Equals(Vertex v1, Vertex v2) + { + if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) + return true; + else + return false; + } + public int GetHashCode(Vertex v) + { + int a = v.X.GetHashCode(); + int b = v.Y.GetHashCode(); + int c = v.Z.GetHashCode(); + return (a << 16) ^ (b << 8) ^ c; + } + + } public Mesh() { - m_vertices = new Dictionary(); + vertexcomp vcomp = new vertexcomp(); + + m_vertices = new Dictionary(vcomp); m_triangles = new List(); + _centroid = Vector3.Zero; + _centroidDiv = 0; } public Mesh Clone() @@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing { result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); } - + result._centroid = _centroid; + result._centroidDiv = _centroidDiv; return result; } @@ -71,15 +97,57 @@ namespace OpenSim.Region.Physics.Meshing throw new NotSupportedException("Attempt to Add to a pinned Mesh"); // If a vertex of the triangle is not yet in the vertices list, // add it and set its index to the current index count + // vertex == seems broken + // skip colapsed triangles + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) + || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) + || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) + ) + { + return; + } + + if (m_vertices.Count == 0) + { + _centroidDiv = 0; + _centroid = Vector3.Zero; + } + if (!m_vertices.ContainsKey(triangle.v1)) + { m_vertices[triangle.v1] = m_vertices.Count; + _centroid.X += triangle.v1.X; + _centroid.Y += triangle.v1.Y; + _centroid.Z += triangle.v1.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v2)) + { m_vertices[triangle.v2] = m_vertices.Count; + _centroid.X += triangle.v2.X; + _centroid.Y += triangle.v2.Y; + _centroid.Z += triangle.v2.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v3)) + { m_vertices[triangle.v3] = m_vertices.Count; + _centroid.X += triangle.v3.X; + _centroid.Y += triangle.v3.Y; + _centroid.Z += triangle.v3.Z; + _centroidDiv++; + } m_triangles.Add(triangle); } + public Vector3 GetCentroid() + { + if (_centroidDiv > 0) + return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv); + else + return Vector3.Zero; + } + public void CalcNormals() { int iTriangles = m_triangles.Count; diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index 6f6ed7f1fc..c4b245f395 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; + private bool cacheSculptAlphaMaps = true; + private string decodedSculptMapPath = null; private bool useMeshiesPhysicsMesh = false; @@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing IConfig mesh_config = config.Configs["Mesh"]; decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); + cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + cacheSculptAlphaMaps = false; + } + else + cacheSculptAlphaMaps = cacheSculptMaps; + if(mesh_config != null) useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); @@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing { if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) return null; + // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // don't loose it + // primShape.SculptData = Utils.EmptyBytes; } +// primShape.SculptDataLoaded = true; } else { if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) return null; } - - // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // keep compatible primShape.SculptData = Utils.EmptyBytes; int numCoords = coords.Count; @@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); - if (cacheSculptMaps) + if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) + // don't cache images with alpha channel in linux since mono can't load them correctly) { try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs index 740424ef91..b3d9cb62f3 100644 --- a/OpenSim/Region/Physics/Meshing/SculptMap.cs +++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs @@ -58,28 +58,24 @@ namespace PrimMesher if (bmW == 0 || bmH == 0) throw new Exception("SculptMap: bitmap has no data"); - int numLodPixels = lod * 2 * lod * 2; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + bool smallMap = bmW * bmH <= numLodPixels; bool needsScaling = false; - bool smallMap = bmW * bmH <= lod * lod; - width = bmW; height = bmH; - while (width * height > numLodPixels) + while (width * height > numLodPixels * 4) { width >>= 1; height >>= 1; needsScaling = true; } - - try { if (needsScaling) - bm = ScaleImage(bm, width, height, - System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor); + bm = ScaleImage(bm, width, height); } catch (Exception e) @@ -87,7 +83,7 @@ namespace PrimMesher throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); } - if (width * height > lod * lod) + if (width * height > numLodPixels) { width >>= 1; height >>= 1; @@ -144,15 +140,17 @@ namespace PrimMesher int rowNdx, colNdx; int smNdx = 0; + for (rowNdx = 0; rowNdx < numRows; rowNdx++) { List row = new List(numCols); for (colNdx = 0; colNdx < numCols; colNdx++) { + if (mirror) - row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); else - row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); ++smNdx; } @@ -161,23 +159,39 @@ namespace PrimMesher return rows; } - private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight, - System.Drawing.Drawing2D.InterpolationMode interpMode) + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) { - Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight); - scaledImage.SetResolution(96.0f, 96.0f); - Graphics grPhoto = Graphics.FromImage(scaledImage); - grPhoto.InterpolationMode = interpMode; + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + float xscale = srcImage.Width / destWidth; + float yscale = srcImage.Height / destHeight; + + float sy = 0.5f; + for (int y = 0; y < destHeight; y++) + { + float sx = 0.5f; + for (int x = 0; x < destWidth; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } - grPhoto.DrawImage(srcImage, - new Rectangle(0, 0, destWidth, destHeight), - new Rectangle(0, 0, srcImage.Width, srcImage.Height), - GraphicsUnit.Pixel); - - grPhoto.Dispose(); + sx += xscale; + } + sy += yscale; + } + srcImage.Dispose(); return scaledImage; } + + } + } -} #endif diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs new file mode 100644 index 0000000000..d46341b482 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs @@ -0,0 +1,58 @@ +/* + * 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.Reflection; +using System.Runtime.InteropServices; + +// Information about this assembly is defined by the following +// attributes. +// +// change them to the information which is associated with the assembly +// you compile. + +[assembly : AssemblyTitle("OdePlugin")] +[assembly : AssemblyDescription("Ubit Variation")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("OdePlugin")] +[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] +[assembly : AssemblyTrademark("")] +[assembly : AssemblyCulture("")] + +// This sets the default COM visibility of types in the assembly to invisible. +// If you need to expose a type to COM, use [ComVisible(true)] on that type. + +[assembly : ComVisible(false)] + +// The assembly version has following format : +// +// Major.Minor.Build.Revision +// +// You can specify all values by your own or you can build default build and revision +// numbers with the '*' character (the default): + +[assembly : AssemblyVersion("0.6.5.*")] diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs new file mode 100644 index 0000000000..793e281894 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -0,0 +1,1451 @@ +/* + * 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. + */ + + +// Revision by Ubit 2011/12 + +using System; +using System.Collections.Generic; +using System.Reflection; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public enum dParam : int + { + LowStop = 0, + HiStop = 1, + Vel = 2, + FMax = 3, + FudgeFactor = 4, + Bounce = 5, + CFM = 6, + StopERP = 7, + StopCFM = 8, + LoStop2 = 256, + HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, + StopERP2 = 7 + 256, + StopCFM2 = 8 + 256, + LoStop3 = 512, + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515, + StopERP3 = 7 + 512, + StopCFM3 = 8 + 512 + } + + public class OdeCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private Vector3 _zeroPosition; + private bool _zeroFlag = false; + private Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + public float PID_D = 800.0f; + public float PID_P = 900.0f; + //private static float POSTURE_SERVO = 10000.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private int m_requestedUpdateFrequency = 0; + public uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public float MinimumGroundFlightOffset = 3f; + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + // other filter control + int m_colliderfilter = 0; + // int m_colliderGroundfilter = 0; + int m_colliderObjectfilter = 0; + + // Default we're a Character + private CollisionCategories m_collisionCategories = (CollisionCategories.Character); + + // Default, Collide with Other Geometries, spaces, bodies and characters. + private CollisionCategories m_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + // we do land collisions not ode | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private OdeScene _parent_scene; + public IntPtr Shell = IntPtr.Zero; + public IntPtr Amotor = IntPtr.Zero; + public d.Mass ShellMass; +// public bool collidelock = false; + + private bool m_haseventsubscription = false; + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + public ContactData AvatarContactData = new ContactData(10f, 0.3f); + + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + + if (pos.IsFinite()) + { + if (pos.Z > 99999f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (pos.Z < -100f) // shouldn't this be 0 ? + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + _position = pos; + } + else + { + _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + _parent_scene = parent_scene; + + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_density = density; + m_mass = 80f; // sure we have a default + + AvatarContactData.mu = parent_scene.AvatarFriction; + AvatarContactData.bounce = parent_scene.AvatarBounce; + + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + + m_isPhysical = false; // current status: no ODE information exists + + m_name = avName; + + AddChange(changes.Add, null); + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override ContactData ContactData + { + get { return AvatarContactData; } + } + + public override bool Building { get; set; } + + /// + /// If this is set, the avatar will move faster + /// + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override bool FloatOnWater + { + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return flying; } + set + { + flying = value; + // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); + } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return (m_iscolliding || m_iscollidingGround); } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + { +// SetPidStatus(false); + m_pidControllerActive = true; + m_iscolliding = true; + } + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + /* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } + + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ + } + + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + // Ubit filter this also + if (value) + { + m_colliderObjectfilter += 2; + if (m_colliderObjectfilter > 2) + m_colliderObjectfilter = 2; + } + else + { + m_colliderObjectfilter--; + if (m_colliderObjectfilter < 0) + m_colliderObjectfilter = 0; + } + + if (m_colliderObjectfilter == 0) + m_iscollidingObj = false; + else + m_iscollidingObj = true; + + // m_iscollidingObj = value; +/* + if (m_iscollidingObj) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + */ + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + /// + /// This 'puts' an avatar somewhere in the physics space. + /// Not really a good choice unless you 'know' it's a good + /// spot otherwise you're likely to orbit the avatar. + /// + public override Vector3 Position + { + get { return _position; } + set + { + if (Body == IntPtr.Zero || Shell == IntPtr.Zero) + { + if (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -100f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + AddChange(changes.Position, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + } + } + } + } + + public override Vector3 RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + /// + /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight + /// and use it to offset landings properly + /// + public override Vector3 Size + { + get { + float d = CAPSULE_RADIUS * 2; + return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // + /// + /// Uses the capped cyllinder volume formula to calculate the avatar's mass. + /// This may be used in calculations in the scene/scenepresence + /// + public override float Mass + { + get + { + float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); + return m_density * AVvolume; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(Vector3 axis) + { + + } + + + public override Vector3 Force + { + get { return _target_velocity; } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void VehicleFlags(int param, bool remove) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override Vector3 CenterOfMass + { + get + { + Vector3 pos = _position; + return pos; + } + } + + public override Vector3 GeometricCenter + { + get + { + Vector3 pos = _position; + return pos; + } + } + + //UBit mess + /* for later use + public override Vector3 PrimOOBsize + { + get + { + Vector3 s=Size; + s.X *=0.5f; + s.Y *=0.5f; + s.Z *=0.5f; + return s; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return Vector3.Zero; + } + } + */ + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 Velocity + { + get + { + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); + } + } + } + + public override Vector3 Torque + { + get { return Vector3.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set + { + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public void SetAcceleration(Vector3 accel) + { + m_pidControllerActive = true; + _acceleration = accel; + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if (pushforce) + { + AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f); + } + else + { + AddChange(changes.Velocity, force); + } + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force applied to a Character"); + } + //m_lastUpdateSent = false; + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + + } + + public override void SetMomentum(Vector3 momentum) + { + } + + + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + + m_mass = ShellMass.mass; // update mass + + // rescale PID parameters + PID_D = _parent_scene.avPIDD; + PID_P = _parent_scene.avPIDP; + + // rescale PID parameters so that this aren't affected by mass + // and so don't get unstable for some masses + // also scale by ode time step so you don't need to refix them + + PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps + PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_P /= 50 * 80; + PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(Shell, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geometry + if (Shell != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep, List defects) + { + // 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 (Body == IntPtr.Zero) + return; + + d.Vector3 dtmp; + d.BodyCopyPosition(Body, out dtmp); + Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + // the Amotor still lets avatar rotation to drift during colisions + // so force it back to identity + + d.Quaternion qtmp; + qtmp.W = 1; + qtmp.X = 0; + qtmp.Y = 0; + qtmp.Z = 0; + d.BodySetQuaternion(Body, ref qtmp); + + if (m_pidControllerActive == false) + { + _zeroPosition = localpos; + } + //PidStatus = true; + + + if (!localpos.IsFinite()) + { + + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + + return; + } + + Vector3 vec = Vector3.Zero; + dtmp = d.BodyGetLinearVel(Body); + Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + float movementdivisor = 1f; + //Ubit change divisions into multiplications below + if (!m_alwaysRun) + { + movementdivisor = 1 / walkDivisor; + } + else + { + movementdivisor = 1 / runDivisor; + } + + // colide with land + + d.AABB aabb; + d.GeomGetAABB(Shell, out aabb); + float chrminZ = aabb.MinZ; + + Vector3 posch = localpos; + + float ftmp; + + if (flying) + { + ftmp = timeStep; + posch.X += vel.X * ftmp; + posch.Y += vel.Y * ftmp; + } + + float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + if (chrminZ < terrainheight) + { + float depth = terrainheight - chrminZ; + if (!flying) + { + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; + } + else + vec.Z = depth * PID_P * 50; + + /* + Vector3 vtmp; + vtmp.X = _target_velocity.X * timeStep; + vtmp.Y = _target_velocity.Y * timeStep; + // fake and avoid squares + float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y)); + if (k > 0) + { + posch.X += vtmp.X; + posch.Y += vtmp.Y; + terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + k = 1 + Math.Abs(terrainheight) / k; + movementdivisor /= k; + + if (k < 1) + k = 1; + } + */ + + + if (depth < 0.1f) + { + m_iscolliding = true; + m_colliderfilter = 2; + m_iscollidingGround = true; + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = chrminZ; + contact.SurfaceNormal.X = 0f; + contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.Z = -1f; + AddCollisionEvent(0, contact); + + vec.Z *= 0.5f; + } + + else + m_iscollidingGround = false; + } + else + m_iscollidingGround = false; + + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f + && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = localpos; + } + if (m_pidControllerActive) + { + // 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 + + vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); + vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); + if (flying) + { + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + _zeroFlag = false; + + if (m_iscolliding) + { + if (!flying) + { + if (_target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. JUMPING + vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + } + // We're standing on something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); + } + else + { + // We're flying and colliding with something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + } + else // ie not colliding + { + if (flying) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + + else + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + } + } + } + + if (flying) + { + vec.Z -= _parent_scene.gravityz * m_mass; + + //Added for auto fly height. Kitto Flora + float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; + + if (localpos.Z < target_altitude) + { + vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; + } + // end add Kitto Flora + } + + if (vec.IsFinite()) + { + if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) + d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (Body == IntPtr.Zero) + return; + + d.Vector3 vec; + try + { + d.BodyCopyPosition(Body, out vec); + } + catch (NullReferenceException) + { + bad = true; + _parent_scene.BadCharacter(this); + vec = new d.Vector3(_position.X, _position.Y, _position.Z); + base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! + m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid); + } + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + bool fixbody = false; + + if (_position.X < 0.0f) + { + fixbody = true; + _position.X = 0.1f; + } + else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + _position.X = (int)_parent_scene.WorldExtents.X - 0.1f; + } + + if (_position.Y < 0.0f) + { + fixbody = true; + _position.Y = 0.1f; + } + else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f; + } + + if (fixbody) + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + // Did we move last? = zeroflag + // This helps keep us from sliding all over +/* + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); + } + } + else + { + m_lastUpdateSent = false; + */ + try + { + vec = d.BodyGetLinearVel(Body); + } + catch (NullReferenceException) + { + vec.X = _velocity.X; + vec.Y = _velocity.Y; + vec.Z = _velocity.Z; + } + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + _velocity.Z = (vec.Z); + // } + } + + /// + /// Cleanup the things we use in the scene. + /// + public void Destroy() + { + AddChange(changes.Remove, null); + } + + public override void CrossingFailure() + { + } + + public override Vector3 PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight { set { return; } } + public override bool PIDHoverActive { set { return; } } + public override PIDHoverType PIDHoverType { set { return; } } + public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + + public override void SubscribeEvents(int ms) + { + m_requestedUpdateFrequency = ms; + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + m_haseventsubscription = true; + } + + public override void UnSubscribeEvents() + { + m_haseventsubscription = false; + _parent_scene.RemoveCollisionEventReporting(this); + m_requestedUpdateFrequency = 0; + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (m_haseventsubscription) + { + // m_log.DebugFormat( + // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + } + + public void SendCollisions() + { + if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency) + { + if (CollisionEventsThisFrame != null) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + } + CollisionEventsThisFrame = new CollisionEventUpdate(); + m_eventsubscription = 0; + } + } + + public override bool SubscribedEvents() + { + return m_haseventsubscription; + } + + private void changePhysicsStatus(bool NewStatus) + { + if (NewStatus != m_isPhysical) + { + if (NewStatus) + { + // Create avatar capsule and related ODE data + if ((Shell != IntPtr.Zero)) + { + // a lost shell ? + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell != IntPtr.Zero ? "Shell " : "") + + (Body != IntPtr.Zero ? "Body " : "") + + (Amotor != IntPtr.Zero ? "Amotor " : "")); + AvatarGeomAndBodyDestroy(); + } + + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + _parent_scene.AddCharacter(this); + } + else + { + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + + m_isPhysical = NewStatus; + } + } + + private void changeAdd() + { + changePhysicsStatus(true); + } + + private void changeRemove() + { + changePhysicsStatus(false); + } + + private void changeShape(PrimitiveBaseShape arg) + { + } + + private void changeSize(Vector3 Size) + { + if (Size.IsFinite()) + { + float caplen = Size.Z; + + caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f; + + if (caplen != CAPSULE_LENGTH) + { + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + AvatarGeomAndBodyDestroy(); + + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = caplen; + + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + + Velocity = Vector3.Zero; + + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == IntPtr.Zero ? "Shell " : "") + + (Body == IntPtr.Zero ? "Body " : "") + + (Amotor == IntPtr.Zero ? "Amotor " : "")); + } + } + + m_pidControllerActive = true; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + + private void changePosition( Vector3 newPos) + { + if (Body != IntPtr.Zero) + d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + + private void changeOrientation(Quaternion newOri) + { + } + + private void changeVelocity(Vector3 newVel) + { + m_pidControllerActive = true; + _target_velocity = newVel; + } + + private void changeSetTorque(Vector3 newTorque) + { + } + + private void changeAddForce(Vector3 newForce) + { + } + + private void changeAddAngularForce(Vector3 arg) + { + } + + private void changeAngularLock(Vector3 arg) + { + } + + private void changeFloatOnWater(bool arg) + { + } + + private void changeVolumedetetion(bool arg) + { + } + + private void changeSelectedStatus(bool arg) + { + } + + private void changeDisable(bool arg) + { + } + + private void changeBuilding(bool arg) + { + } + + private void changeForce(Vector3 newForce) + { + m_pidControllerActive = false; + if (Body != IntPtr.Zero) + { + if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) + d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); + } + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeAdd(); + break; + case changes.Remove: + changeRemove(); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changeVelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; +/* not in use for now + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; +*/ + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor)this, what, arg); + } + + + internal void AddCollisionFrameTime(int p) + { + // protect it from overflow crashing + if (m_eventsubscription + p >= int.MaxValue) + m_eventsubscription = 0; + m_eventsubscription += p; + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000000..80218e74cb --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -0,0 +1,849 @@ +/* + * 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. + */ + +/* 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. + */ + +// Ubit 2012 + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + private OdePrim rootPrim; + private OdeScene _pParentScene; + + // Vehicle properties + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? + + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + + 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 + private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + private float m_linearMotorDecayTimescale = 120; + private float m_linearMotorTimescale = 1000; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + + //Deflection properties + private float m_angularDeflectionEfficiency = 0; + private float m_angularDeflectionTimescale = 1000; + private float m_linearDeflectionEfficiency = 0; + private float m_linearDeflectionTimescale = 1000; + + //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 = 1000f; + 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 = 1000f; // Timescale > 300 means no vert attractor. + + // auxiliar + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + + private float m_lmEfect = 0; // current linear motor eficiency + private float m_amEfect = 0; // current angular motor eficiency + + + public ODEDynamics(OdePrim rootp) + { + rootPrim = rootp; + _pParentScene = rootPrim._parent_scene; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; + else if (pValue > 120) pValue = 120; + m_angularMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + 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 < timestep) pValue = timestep; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + 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: + if (pValue < timestep) pValue = timestep; + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + 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 + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + 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); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + case Vehicle.BLOCK_EXIT: + m_BlockingEndPoint = 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 = Quaternion.Inverse(pValue); + break; + case Vehicle.ROLL_FRAME: + m_RollreferenceFrame = pValue; + break; + } + }//end ProcessRotationVehicleParam + + internal void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + internal void ProcessTypeChange(Vehicle pType) + { + float invtimestep = _pParentScene.ODE_STEPSIZE; + m_lmEfect = 0; + m_amEfect = 0; + + m_linearMotorDirection = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + + m_BlockingEndPoint = Vector3.Zero; + m_RollreferenceFrame = Quaternion.Identity; + m_linearMotorOffset = Vector3.Zero; + + m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * invtimestep; + m_VhoverHeight = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 1; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 10; + 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_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f * invtimestep; + 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_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4 * invtimestep; + 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_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_ROLL_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 0.5f; + m_angularDeflectionEfficiency = 1; + m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + m_bankingEfficiency = 1; + m_bankingMix = 0.7f; + m_bankingTimescale = 2; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + 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_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10 * invtimestep; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 5 * invtimestep; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0f; + m_verticalAttractionTimescale = 1000f; + m_bankingEfficiency = 0; + m_bankingMix = 0.7f; + m_bankingTimescale = 5; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + } + + }//end SetDefaultsForType + + internal void Stop() + { + m_lmEfect = 0; + m_amEfect = 0; + } + + public static Vector3 Xrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; + vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); + vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); + return vec; + } + + public static Vector3 Zrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); + vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); + vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; + + return vec; + } + + private const float halfpi = 0.5f * (float)Math.PI; + + public static Vector3 ubitRot2Euler(Quaternion rot) + { + // returns roll in X + // pitch in Y + // yaw in Z + Vector3 vec; + + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + vec.X = 0; + vec.Y = -halfpi; + vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); + } + else if (zX > 0.49999f) + { + vec.X = 0; + vec.Y = halfpi; + vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); + } + else + { + vec.Y = (float)Math.Asin(2 * zX); + + float sqw = rot.W * rot.W; + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + sqw - 0.5f; + + vec.X = (float)Math.Atan2(minuszY, zZ); + + float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) + float yY = rot.X * rot.X + sqw - 0.5f; + vec.Z = (float)Math.Atan2(yX, yY); + } + return vec; + } + + public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) + { + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + roll = 0; + pitch = -halfpi; + } + else if (zX > 0.49999f) + { + roll = 0; + pitch = halfpi; + } + else + { + pitch = (float)Math.Asin(2 * zX); + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; + + roll = (float)Math.Atan2(minuszY, zZ); + } + return ; + } + + internal void Step()//float pTimestep) + { + IntPtr Body = rootPrim.Body; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion rotq = objrotq; // rotq = rotation of object + rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame + Quaternion irotq = Quaternion.Inverse(rotq); + + d.Vector3 dvtmp; + Vector3 tmpV; + Vector3 curVel; // velocity in world + Vector3 curAngVel; // angular velocity in world + Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame + Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame + d.Vector3 dtorque = new d.Vector3(); + + dvtmp = d.BodyGetLinearVel(Body); + curVel.X = dvtmp.X; + curVel.Y = dvtmp.Y; + curVel.Z = dvtmp.Z; + Vector3 curLocalVel = curVel * irotq; // current velocity in local + + dvtmp = d.BodyGetAngularVel(Body); + curAngVel.X = dvtmp.X; + curAngVel.Y = dvtmp.Y; + curAngVel.Z = dvtmp.Z; + Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local + + // linear motor + if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) + { + tmpV = m_linearMotorDirection - curLocalVel; // velocity error + tmpV *= m_lmEfect / m_linearMotorTimescale; // error to correct in this timestep + tmpV *= rotq; // to world + + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) + tmpV.Z = 0; + + if (m_linearMotorOffset.X != 0 || m_linearMotorOffset.Y != 0 || m_linearMotorOffset.Z != 0) + { + // have offset, do it now + tmpV *= rootPrim.Mass; + d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); + } + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale); + } + else + m_lmEfect = 0; + + // friction + if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) + { + tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; + tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; + tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; + tmpV *= rotq; // to world + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + // hover + if (m_VhoverTimescale < 300) + { + d.Vector3 pos = d.BodyGetPosition(Body); + + // default to global + float perr = m_VhoverHeight - pos.Z;; + + if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + } + else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + { + force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; + } + else + { + // default gravity and buoancy + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + + // linear deflection + if (m_linearDeflectionEfficiency > 0) + { + float len = curVel.Length(); + Vector3 atAxis; + atAxis = Xrot(rotq); // where are we pointing to + atAxis *= len; // make it same size as world velocity vector + tmpV = -atAxis; // oposite direction + atAxis -= curVel; // error to one direction + len = atAxis.LengthSquared(); + tmpV -= curVel; // error to oposite + float lens = tmpV.LengthSquared(); + if (len > 0.01 || lens > 0.01) // do nothing if close enougth + { + if (len < lens) + tmpV = atAxis; + + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) + force.Z += tmpV.Z; + } + } + + // angular motor + if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) + { + tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error + tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; + m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); + } + else + m_amEfect = 0; + + // angular friction + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X; + torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y; + torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z; + } + + // angular deflection + if (m_angularDeflectionEfficiency > 0) + { + Vector3 dirv; + + if (curLocalVel.X > 0.01f) + dirv = curLocalVel; + else if (curLocalVel.X < -0.01f) + // use oposite + dirv = -curLocalVel; + else + { + // make it fall into small positive x case + dirv.X = 0.01f; + dirv.Y = curLocalVel.Y; + dirv.Z = curLocalVel.Z; + } + + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; + + if (Math.Abs(dirv.Z) > 0.01) + { + torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; + } + + if (Math.Abs(dirv.Y) > 0.01) + { + torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; + } + } + + // vertical atractor + if (m_verticalAttractionTimescale < 300) + { + float roll; + float pitch; + + GetRollPitch(irotq, out roll, out pitch); + + float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; + float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + + if (Math.Abs(roll) > 0.01) // roll + { + torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2; + } + + if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch + { + torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2; + } + + if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01) + { + float broll = roll * m_bankingEfficiency; ; + if (m_bankingMix != 0) + { + float vfact = Math.Abs(curLocalVel.X) / 10.0f; + if (vfact > 1.0f) vfact = 1.0f; + if (curLocalVel.X >= 0) + broll *= ((1 - m_bankingMix) + vfact); + else + broll *= -((1 - m_bankingMix) + vfact); + } + broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; + // torque.Z += broll; + + // make z rot be in world Z not local as seems to be in sl + tmpV.X = 0; + tmpV.Y = 0; + tmpV.Z = broll; + tmpV *= irotq; + + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; + } + } + + d.Mass dmass; + d.BodyGetMass(Body,out dmass); + + if (force.X != 0 || force.Y != 0 || force.Z != 0) + { + force *= dmass.mass; + d.BodySetForce(Body, force.X, force.Y, force.Z); + } + + if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) + { + torque *= m_referenceFrame; // to object frame + dtorque.X = torque.X; + dtorque.Y = torque.Y; + dtorque.Z = torque.Z; + + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); + d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame + } + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs new file mode 100644 index 0000000000..3b7f562047 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -0,0 +1,3317 @@ +/* + * 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. + */ + +/* Revision 2011/12 by Ubit Umarov + * + * + */ + +/* + * 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. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + + protected bool m_building; + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + public bool m_taintselected; + + public uint m_localID; + + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + private bool m_wascolliding; + private bool m_isSelected; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public int m_interpenetrationcount; + public float m_collisionscore; + int m_colliderfilter = 0; + public int m_roundsUnderMotionThreshold; + private int m_crossingfailures; + + public bool outofBounds; + private float m_density = 10.000006836f; // Aluminum g/cm3; + + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + public d.Mass objectpMass; // object last computed inertia + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + private IntPtr m_linkJoint = IntPtr.Zero; + private IntPtr _linkJointGroup = IntPtr.Zero; + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f }; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } + } + + public override ContactData ContactData + { + get + { + return primContactData; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if (value) + m_isSelected = value; + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + m_iscolliding = true; + + if (m_wascolliding != m_iscolliding) + { + if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero) + d.BodyEnable(Body); + m_wascolliding = m_iscolliding; + } + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return _mass; } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc, (param != 0)); + } + + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } + + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { + AddChange(changes.Shape, value); + } + } + + public override Vector3 Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); + // _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori > 0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + // we may need to put a fake on this + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + AddChange(changes.VehicleType, value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleRotationParam(int param, Quaternion value) + { + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + if (m_vehicle == null) + return; + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + m_crossingfailures++; + changeDisable(false); + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu; + primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) + { + Name = primName; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_wascolliding = false; + m_colliderfilter = 0; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + + primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.BodyGetQuaternion(Body); + Quaternion curr; // crap convertion between identical things + curr.X = dcur.X; + curr.Y = dcur.Y; + curr.Z = dcur.Z; + curr.W = dcur.W; + Vector3 ax; + + const int StopERP = 7; + const int StopCFM = 8; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f); + i++; + j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this, false); + } + } + else + { + DestroyBody(); + } + } + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true); + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); + _size.X = 0.01f; + _size.Y = 0.01f; + _size.Z = 0.01f; + return false; + } + + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; + + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + mesh.releaseSourceMeshData(); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + return false; + } + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + + /* + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + //Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + */ + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + } + + if (!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; + CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + if (m_targetSpace != _parent_scene.ActiveSpace) + { + m_targetSpace = _parent_scene.ActiveSpace; + + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom != IntPtr.Zero) + { + d.SpaceAdd(m_targetSpace, prm.prim_geom); + prm.m_targetSpace = m_targetSpace; + } + } + d.SpaceAdd(m_targetSpace, prim_geom); + } + d.GeomEnable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomEnable(prm.prim_geom); + + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + if (m_targetSpace == _parent_scene.ActiveSpace) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero) + { + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + prm.m_targetSpace = IntPtr.Zero; + } + } + d.SpaceRemove(m_targetSpace, prim_geom); + m_targetSpace = IntPtr.Zero; + } + d.GeomDisable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomDisable(prm.prim_geom); + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation and position + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .001f, .0002f); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + + m_disabled = false; + + if (m_targetSpace != _parent_scene.ActiveSpace) + { + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + + if (prm.m_targetSpace != _parent_scene.ActiveSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + d.GeomEnable(prm.prim_geom); + prm.m_disabled = false; + prm.m_interpenetrationcount = 0; + prm.m_collisionscore = 0; + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + d.GeomEnable(prim_geom); + m_disabled = false; + _parent_scene.addActivePrim(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prm.prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_disabled = true; + m_collisionscore = 0; + } + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if (prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + if (!m_isphysical) + SetInStaticSpace(this); + } + + if (m_isphysical && Body == IntPtr.Zero) + { + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + { + */ + MakeBody(); + // } + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this, false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if (!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + + } + } + + private void changeSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + disableBodySoft(); + } + else + { + m_collisionCategories = CollisionCategories.Geom; + + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags; + + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + enableBodySoft(); + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + // changeSelectedStatus(); + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + { + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + { + MakeBody(); + } + } + } + else + { + if (Body != IntPtr.Zero) + { + // UpdateChildsfromgeom(); + /* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + DestroyBody(); + Stop(); + } + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + DestroyBody(); + } + else + { + m_building = false; + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + private void changeVehicleType(int value) + { + if (m_vehicle == null) + { + if (value != (int)Vehicle.TYPE_NONE) + { + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + else + m_vehicle.ProcessTypeChange((Vehicle)value); + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. + { + // if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building) + { + if (Body != IntPtr.Zero) + { + if (m_crossingfailures != 0 && m_crossingfailures < 5) + { + _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f); + _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y); + if (_position.Z < tmp) + _position.Z = tmp + 0.2f; + + m_lastposition = _position; + m_lastorientation = _orientation; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + m_rotationalVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + m_crossingfailures = 0; // do this only once + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + enableBodySoft(); + base.RequestPhysicsterseUpdate(); + return; + } + + else if (m_crossingfailures != 0) + { + return; + } + + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator + + // we need to use root position since that's all the rest of scene uses + if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X + || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // we can't let it keeping moving and having colisions + // since it can be stucked between something like terrain and edge + // so lets stop and disable it until something else kicks it + if (m_crossingfailures == 0) + { + + _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f); + _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f); + _position.Z = Util.Clip(lpos.Z, -100f, 50000f); + + m_lastposition = _position; + m_lastorientation = _orientation; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + m_crossingfailures++; // do this only once + base.RequestPhysicsterseUpdate(); + return; + } + } + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + lpos.Z = Util.Clip(lpos.Z, -100f, 50000f); + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + + return; + } + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this, false); + } + else + ChildRemove(this, false); + + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor) this, what, arg); + } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs new file mode 100644 index 0000000000..4b3f83b24d --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -0,0 +1,443 @@ +/* + * 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 System.Text; +using OpenMetaverse; +using OpenSim.Region.Physics.Manager; +using OdeAPI; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Processes raycast requests as ODE is in a state to be able to do them. + /// This ensures that it's thread safe and there will be no conflicts. + /// Requests get returned by a different thread then they were requested by. + /// + public class ODERayCastRequestManager + { + /// + /// Pending ray requests + /// + protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); + + /// + /// Scene that created this object. + /// + private OdeScene m_scene; + + IntPtr ray; + + private const int ColisionContactGeomsPerTest = 5; + + /// + /// ODE near callback delegate + /// + private d.NearCallback nearCallback; + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private List m_contactResults = new List(); + + public ODERayCastRequestManager(OdeScene pScene) + { + m_scene = pScene; + nearCallback = near; + ray = d.CreateRay(IntPtr.Zero, 1.0f); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + int time = System.Environment.TickCount; + + if (m_PendingRequests.Count <= 0) + return 0; + + if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still + { + m_PendingRequests.Clear(); + return 0; + } + + ODERayRequest req; + + int i = 50; // arbitary limit of processed tests per frame + + while(m_PendingRequests.Dequeue(out req)) + { + if (req.geom == IntPtr.Zero) + doSpaceRay(req); + else + doGeomRay(req); + if(--i < 0) + break; + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return System.Environment.TickCount - time; + } + /// + /// Method that actually initiates the raycast with full top space + /// + /// + private void doSpaceRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback); + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + /// + /// Method that actually initiates the raycast with a geom + /// + /// + private void doGeomRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; + if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + // This is the standard Near. g2 is the ray + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + //Don't test against heightfield Geom, or you'll be sorry! + // Exclude heightfield geom + + if (g1 == IntPtr.Zero || g1 == g2) + return; + + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + return; + + // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. + if (d.GeomIsSpace(g1)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS Ray]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; + + PhysicsActor p1 = null; + + if (g1 != IntPtr.Zero) + m_scene.actor_name_map.TryGetValue(g1, out p1); + + d.ContactGeom curcontact = new d.ContactGeom(); + // Loop over contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + if (p1 != null) { + if (p1 is OdePrim) + { + ContactResult collisionresult = new ContactResult(); + + collisionresult.ConsumerID = ((OdePrim)p1).m_localID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + } + } + + /// + /// Dereference the creator scene so that it can be garbage collected if needed. + /// + internal void Dispose() + { + m_scene = null; + } + } + + public struct ODERayRequest + { + public IntPtr geom; + public Vector3 Origin; + public Vector3 Normal; + public int Count; + public float length; + public object callbackMethod; + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs new file mode 100644 index 0000000000..e62746e59e --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -0,0 +1,1961 @@ + +/* + * based on: + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +All rights reserved. + +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 names of ODE's copyright owner 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 COPYRIGHT HOLDERS AND CONTRIBUTORS +"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 COPYRIGHT +OWNER OR 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. + * + * changes by opensim team; + * changes by Aurora team http://www.aurora-sim.org/ + + * Revision/fixs by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; + +namespace OdeAPI +{ +//#if dDOUBLE +// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim +// at least we save same memory and memory access time, FPU performance on intel usually is similar +// using dReal = System.Double; +//#else + using dReal = System.Single; +//#endif + + public static class d + { + public static dReal Infinity = dReal.MaxValue; + public static int NTotalBodies = 0; + public static int NTotalGeoms = 0; + + #region Flags and Enumerations + + [Flags] + public enum AllocateODEDataFlags : uint + { + BasicData = 0, + CollisionData = 0x00000001, + All = ~0u + } + + [Flags] + public enum IniteODEFlags : uint + { + dInitFlagManualThreadCleanup = 0x00000001 + } + + [Flags] + public enum ContactFlags : int + { + Mu2 = 0x001, + FDir1 = 0x002, + Bounce = 0x004, + SoftERP = 0x008, + SoftCFM = 0x010, + Motion1 = 0x020, + Motion2 = 0x040, + MotionN = 0x080, + Slip1 = 0x100, + Slip2 = 0x200, + Approx0 = 0x0000, + Approx1_1 = 0x1000, + Approx1_2 = 0x2000, + Approx1 = 0x3000 + } + + public enum GeomClassID : int + { + SphereClass, + BoxClass, + CapsuleClass, + CylinderClass, + PlaneClass, + RayClass, + ConvexClass, + GeomTransformClass, + TriMeshClass, + HeightfieldClass, + FirstSpaceClass, + SimpleSpaceClass = FirstSpaceClass, + HashSpaceClass, + QuadTreeSpaceClass, + LastSpaceClass = QuadTreeSpaceClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 4 + } + + public enum JointType : int + { + None, + Ball, + Hinge, + Slider, + Contact, + Universal, + Hinge2, + Fixed, + Null, + AMotor, + LMotor, + Plane2D + } + + public enum JointParam : int + { + LoStop, + HiStop, + Vel, + FMax, + FudgeFactor, + Bounce, + CFM, + StopERP, + StopCFM, + SuspensionERP, + SuspensionCFM, + LoStop2 = 256, + HiStop2, + Vel2, + FMax2, + FudgeFactor2, + Bounce2, + CFM2, + StopERP2, + StopCFM2, + SuspensionERP2, + SuspensionCFM2, + LoStop3 = 512, + HiStop3, + Vel3, + FMax3, + FudgeFactor3, + Bounce3, + CFM3, + StopERP3, + StopCFM3, + SuspensionERP3, + SuspensionCFM3 + } + + public enum dSweepAndPruneAxis : int + { + XYZ = ((0)|(1<<2)|(2<<4)), + XZY = ((0)|(2<<2)|(1<<4)), + YXZ = ((1)|(0<<2)|(2<<4)), + YZX = ((1)|(2<<2)|(0<<4)), + ZXY = ((2)|(0<<2)|(1<<4)), + ZYX = ((2)|(1<<2)|(0<<4)) + } + + #endregion + + #region Callbacks + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GetAABBFn(IntPtr geom, out AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate ColliderFn GetColliderFnFn(int num); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GeomDtorFn(IntPtr o); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); + + #endregion + + #region Structs + + [StructLayout(LayoutKind.Sequential)] + public struct AABB + { + public dReal MinX, MaxX; + public dReal MinY, MaxY; + public dReal MinZ, MaxZ; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Contact + { + public SurfaceParameters surface; + public ContactGeom geom; + public Vector3 fdir1; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); + } + + + [StructLayout(LayoutKind.Sequential)] + public struct ContactGeom + { + + public Vector3 pos; + public Vector3 normal; + public dReal depth; + public IntPtr g1; + public IntPtr g2; + public int side1; + public int side2; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); + } + + [StructLayout(LayoutKind.Sequential)] + public struct GeomClass + { + public int bytes; + public GetColliderFnFn collider; + public GetAABBFn aabb; + public AABBTestFn aabb_test; + public GeomDtorFn dtor; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct JointFeedback + { + public Vector3 f1; + public Vector3 t1; + public Vector3 f2; + public Vector3 t2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Mass + { + public dReal mass; + public Vector4 c; + public Matrix3 I; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3 + { + public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) + { + M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + } + public dReal M00, M10, M20; + private dReal _m30; + public dReal M01, M11, M21; + private dReal _m31; + public dReal M02, M12, M22; + private dReal _m32; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, + dReal m01, dReal m11, dReal m21, dReal m31, + dReal m02, dReal m12, dReal m22, dReal m32, + dReal m03, dReal m13, dReal m23, dReal m33) + { + M00 = m00; M10 = m10; M20 = m20; M30 = m30; + M01 = m01; M11 = m11; M21 = m21; M31 = m31; + M02 = m02; M12 = m12; M22 = m22; M32 = m32; + M03 = m03; M13 = m13; M23 = m23; M33 = m33; + } + public dReal M00, M10, M20, M30; + public dReal M01, M11, M21, M31; + public dReal M02, M12, M22, M32; + public dReal M03, M13, M23, M33; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public dReal W, X, Y, Z; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct SurfaceParameters + { + public ContactFlags mode; + public dReal mu; + public dReal mu2; + public dReal bounce; + public dReal bounce_vel; + public dReal soft_erp; + public dReal soft_cfm; + public dReal motion1; + public dReal motion2; + public dReal motionN; + public dReal slip1; + public dReal slip2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public Vector3(dReal x, dReal y, dReal z) + { + X = x; Y = y; Z = z; _w = 0.0f; + } + public dReal X, Y, Z; + private dReal _w; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 + { + public Vector4(dReal x, dReal y, dReal z, dReal w) + { + X = x; Y = y; Z = z; W = w; + } + public dReal X, Y, Z, W; + } + + #endregion + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern int AllocateODEDataForThread(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnected(IntPtr b1, IntPtr b2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyiCreate(IntPtr world); + public static IntPtr BodyCreate(IntPtr world) + { + NTotalBodies++; + return BodyiCreate(world); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void BodyiDestroy(IntPtr body); + public static void BodyDestroy(IntPtr body) + { + NTotalBodies--; + BodyiDestroy(body); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDisable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyEnable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetAutoDisableFlag(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetAutoDisableSteps(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableTime(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + public static Vector3 BodyGetAngularVel(IntPtr body) + { + unsafe { return *(BodyGetAngularVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetData(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetFiniteRotationMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); + public static Vector3 BodyGetForce(IntPtr body) + { + unsafe { return *(BodyGetForceUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetGravityMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetGyroscopicMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetJoint(IntPtr body, int index); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + public static Vector3 BodyGetLinearVel(IntPtr body) + { + unsafe { return *(BodyGetLinearVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetMass(IntPtr body, out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetNumJoints(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + public static Vector3 BodyGetPosition(IntPtr body) + { + unsafe { return *(BodyGetPositionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + public static Quaternion BodyGetQuaternion(IntPtr body) + { + unsafe { return *(BodyGetQuaternionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); + public static Matrix3 BodyGetRotation(IntPtr body) + { + unsafe { return *(BodyGetRotationUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); + public static Vector3 BodyGetTorque(IntPtr body) + { + unsafe { return *(BodyGetTorqueUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetWorld(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetFirstGeom(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyIsEnabled(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetData(IntPtr body, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetGravityMode(IntPtr body, bool mode); + + /// + /// Sets the Gyroscopic term status on the body specified. + /// + /// Pointer to body + /// NonZero enabled, Zero disabled + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetMass(IntPtr body, ref Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 p2, + ref Matrix3 R2, ref Vector3 side2, + ref Vector3 normal, out dReal depth, out int return_code, + int maxc, out ContactGeom contact, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 _p2, + ref Matrix3 R2, ref Vector3 side2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern void CleanupODEAllDataForThread(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] + public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, + ref Vector3 b1, ref Vector3 b2, + ref Vector3 cp1, ref Vector3 cp2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] + public static extern void CloseODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); + public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) + { + NTotalGeoms++; + return CreateiBox(space, lx, ly, lz); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCapsule(space, radius, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) + { + NTotalGeoms++; + return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCylinder(space, radius, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiHeightfield(space, data, bPlaceable); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiGeom(int classnum); + public static IntPtr CreateGeom(int classnum) + { + NTotalGeoms++; + return CreateiGeom(classnum); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] + public static extern int CreateGeomClass(ref GeomClass classptr); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomTransform"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeomTransform(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreatePlane"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) + { + NTotalGeoms++; + return CreateiPlane(space, a, b, c, d); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiRay(IntPtr space, dReal length); + public static IntPtr CreateRay(IntPtr space, dReal length) + { + NTotalGeoms++; + return CreateiRay(space, length); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); + public static IntPtr CreateSphere(IntPtr space, dReal radius) + { + NTotalGeoms++; + return CreateiSphere(space, radius); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); + public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) + { + NTotalGeoms++; + return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] + public static extern void DQfromW(dReal[] dq, ref Vector3 w, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorCholesky"), SuppressUnmanagedCodeSecurity] + public static extern int FactorCholesky(ref dReal A00, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dFactorLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void FactorLDLT(ref dReal A, out dReal d, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out Vector3 len); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxGetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxGetLengths(IntPtr geom, out dReal x); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxPointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomBoxPointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomBoxSetLengths"), SuppressUnmanagedCodeSecurity] + public static extern void GeomBoxSetLengths(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsulePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomCapsulePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCapsuleSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCapsuleSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomClearOffset"), SuppressUnmanagedCodeSecurity] + public static extern void GeomClearOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetPosition(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomCopyOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyPosition(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyRotation(IntPtr geom, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderGetParams(IntPtr geom, out dReal radius, out dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomCylinderSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCylinderSetParams(IntPtr geom, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomiDestroy(IntPtr geom); + public static void GeomDestroy(IntPtr geom) + { + NTotalGeoms--; + GeomiDestroy(geom); + } + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDisable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomEnable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out dReal minX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetBody(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCategoryBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetClassData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCollideBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] + public static extern GeomClassID GeomGetClass(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetOffsetPosition(IntPtr geom) + { + unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetOffsetRotation(IntPtr geom) + { + unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetPosition(IntPtr geom) + { + unsafe { return *(GeomGetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetRotation(IntPtr geom) + { + unsafe { return *(GeomGetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsEnabled(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern int GeomRayGetClosestHit(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetLength(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetLength(IntPtr ray, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetBody(IntPtr geom, IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCategoryBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSphereGetRadius(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetCleanup(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTransformGetGeom(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetInfo(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetInfo(IntPtr geom, int info); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + double[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + dReal[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + float[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshClearTCCache(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataPreprocess(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataUpdate(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); + public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) + { + unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] + public extern static int GeomTriMeshGetTriangleCount(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] + public static extern string GetConfiguration(string str); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr HashSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] + public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] + public static extern void InitODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] + public static extern int InitODE2(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] + public static extern int IsPositiveDefinite(ref dReal A, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] + public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddPRTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddSliderForce(IntPtr joint, dReal force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] + public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointDestroy(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorMode(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetBody(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetData(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] + public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); + public static JointFeedback JointGetFeedback(IntPtr j) + { + unsafe { return *(JointGetFeedbackUnsafe(j)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngle(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngleRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetLMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] + public static extern JointType JointGetType(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGroupCreate(int max_size); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupDestroy(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupEmpty(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorMode(IntPtr j, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorNumAxes(IntPtr group, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetData(IntPtr j, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFixed(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorNumAxes(IntPtr j, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] + public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdd(ref Mass a, ref Mass b); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdjust(ref Mass m, dReal newmass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] + public static extern bool MassCheck(ref Mass m); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetParameters(out Mass mass, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal i11, dReal i22, dReal i33, + dReal i12, dReal i13, dReal i23); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] + public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetZero(out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] + public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); + public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) + { + MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] + public static extern void QfromR(out Quaternion q, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void QSetIdentity(out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] + public static extern dReal RandReal(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] + public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] + public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + public static extern void RfromQ(out Matrix3 R, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] + public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void RSetIdentity(out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] + public static extern void SetValue(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void SetZero(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SimpleSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] + public static extern void SolveCholesky(ref dReal L, out dReal b, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceAdd(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceLockQuery(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceClean(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceDestroy(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceGetCleanup(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetNumGeoms(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SpaceGetGeom(IntPtr space, int i); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetSublevel(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceQuery(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceRemove(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetCleanup(IntPtr space, bool mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetSublevel(IntPtr space, int sublevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] + public static extern void VectorScale(out dReal a, ref dReal d, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr WorldCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void WorldDestroy(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool WorldGetAutoDisableFlag(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableSteps(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableTime(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetCFM(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetERP(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetQuickStepNumIterations(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetQuickStepW(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldQuickStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetCFM(IntPtr world, dReal cfm); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetERP(IntPtr world, dReal erp); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] + public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs new file mode 100644 index 0000000000..215d47a7ac --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs @@ -0,0 +1,86 @@ +/* + * 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 System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// ODE plugin + /// + public class OdePlugin : IPhysicsPlugin + { + //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + + private OdeScene m_scene; + + public bool Init() + { + if (d.InitODE2(0) != 0) + { + if (d.AllocateODEDataForThread(~0U) == 0) + { + d.CloseODE(); + return false; + } + return true; + } + return false; + } + + public PhysicsScene GetScene(String sceneIdentifier) + { + if (m_scene == null) + { + m_scene = new OdeScene(sceneIdentifier); + } + return (m_scene); + } + + public string GetName() + { + return ("UbitODE"); + } + + public void Dispose() + { + d.CloseODE(); + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs new file mode 100644 index 0000000000..56f3786b31 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -0,0 +1,2540 @@ +/* + * 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. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public enum StatusIndicators : int + { + Generic = 0, + Start = 1, + End = 2 + } + + public struct sCollisionData + { + public uint ColliderLocalId; + public uint CollidedWithLocalId; + public int NumberOfCollisions; + public int CollisionType; + public int StatusIndicator; + public int lastframe; + } + + [Flags] + public enum CollisionCategories : int + { + Disabled = 0, + Geom = 0x00000001, + Body = 0x00000002, + Space = 0x00000004, + Character = 0x00000008, + Land = 0x00000010, + Water = 0x00000020, + Wind = 0x00000040, + Sensor = 0x00000080, + Selected = 0x00000100 + } + + /// + /// Material type for a primitive + /// + public enum Material : int + { + /// + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6, + + light = 7 // compatibility with old viewers + } + + public enum changes : int + { + Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) + Remove, + Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root + // or removes from a object if arg is null + DeLink, + Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child + Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child + PosOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + OriOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + Velocity, + AngVelocity, + Acceleration, + Force, + Torque, + + AddForce, + AddAngForce, + AngLock, + + Size, + Shape, + + CollidesWater, + VolumeDtc, + + Physical, + Selected, + disabled, + building, + + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + + Null //keep this last used do dim the methods array. does nothing but pulsing the prim + } + + public struct ODEchangeitem + { + public PhysicsActor actor; + public OdeCharacter character; + public changes what; + public Object arg; + } + + public class OdeScene : PhysicsScene + { + private readonly ILog m_log; + // private Dictionary m_storedCollisions = new Dictionary(); + + private int threadid = 0; + private Random fluidRandomizer = new Random(Environment.TickCount); + + const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + const float comumContactERP = 0.6f; + const float comumSoftContactERP = 0.1f; + const float comumContactCFM = 0.0001f; + + float frictionScale = 1.0f; + + float frictionMovementMult = 0.3f; + + float TerrainBounce = 0.3f; + float TerrainFriction = 0.3f; + + public float AvatarBounce = 0.3f; + public float AvatarFriction = 0;// 0.9f * 0.5f; + + private const uint m_regionWidth = Constants.RegionSize; + private const uint m_regionHeight = Constants.RegionSize; + + public float ODE_STEPSIZE = 0.020f; + private float metersInSpace = 25.6f; + private float m_timeDilation = 1.0f; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + private float waterlevel = 0f; + private int framecount = 0; + + internal IntPtr WaterGeom; + + public float avPIDD = 3200f; // make it visible + public float avPIDP = 1400f; // make it visible + private float avCapRadius = 0.37f; + private float avDensity = 3f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; + + public float geomDefaultDensity = 10.000006836f; + + public int geomContactPointsStartthrottle = 3; + public int geomUpdatesPerThrottledUpdate = 15; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int geomCrossingFailuresBeforeOutofbounds = 6; + + public int bodyFramesAutoDisable = 20; + + private float[] _watermap; + private bool m_filterCollisions = true; + + private d.NearCallback nearCallback; + + private readonly HashSet _characters = new HashSet(); + private readonly HashSet _prims = new HashSet(); + private readonly HashSet _activeprims = new HashSet(); + + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + + /// + /// A list of actors that should receive collision events. + /// + private readonly List _collisionEventPrim = new List(); + + private readonly HashSet _badCharacter = new HashSet(); + public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); + + private float contactsurfacelayer = 0.002f; + + private int contactsPerCollision = 80; + internal IntPtr ContactgeomsArray = IntPtr.Zero; + private IntPtr GlobalContactsArray = IntPtr.Zero; + + const int maxContactsbeforedeath = 4000; + private volatile int m_global_contactcount = 0; + + + private readonly IntPtr contactgroup; + + public ContactData[] m_materialContactsData = new ContactData[8]; + + private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); + private readonly Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag + private readonly PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; + + public IntPtr world; + + private uint obj2LocalID = 0; + private OdeCharacter cc1; + private OdePrim cp1; + private OdeCharacter cc2; + private OdePrim cp2; + + // split the spaces acording to contents type + // ActiveSpace contains characters and active prims + // StaticSpace contains land and other that is mostly static in enviroment + // this can contain subspaces, like the grid in staticspace + // as now space only contains this 2 top spaces + + public IntPtr TopSpace; // the global space + public IntPtr ActiveSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeter; + + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + + private Object OdeLock; + private static Object SimulationLock; + + public IMesher mesher; + + private IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + private Vector3 m_worldOffset = Vector3.Zero; + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + private PhysicsScene m_parentScene = null; + + private ODERayCastRequestManager m_rayCastManager; + + +/* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + public OdeScene(string sceneIdentifier) + { + m_log + = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); + +// checkThread(); + Name = sceneIdentifier; + + OdeLock = new Object(); + SimulationLock = new Object(); + + nearCallback = near; + + m_rayCastManager = new ODERayCastRequestManager(this); + lock (OdeLock) + { + // Create the world and the first space + try + { + world = d.WorldCreate(); + TopSpace = d.HashSpaceCreate(IntPtr.Zero); + + // now the major subspaces + ActiveSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + } + + d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does + d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(StaticSpace, -2, 8); + + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + + contactgroup = d.JointGroupCreate(0); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + } + + _watermap = new float[258 * 258]; + } + + // Initialize the mesh plugin +// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { +// checkThread(); + mesher = meshmerizer; + m_config = config; + +// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE")); + /* + if (region != null) + { + WorldExtents.X = region.RegionSizeX; + WorldExtents.Y = region.RegionSizeY; + } + */ + + // Defaults + + avPIDD = 2200.0f; + avPIDP = 900.0f; + + int contactsPerCollision = 80; + + if (m_config != null) + { + IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", 0f); + gravityy = physicsconfig.GetFloat("world_gravityy", 0f); + gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); + geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); + + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + + forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); + m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + } + else + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + } + + physics_logging = physicsconfig.GetBoolean("physics_logging", false); + physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); + physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); + + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + } + } + + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + + m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; + + m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + + m_materialContactsData[(int)Material.light].mu = 0.0f; + m_materialContactsData[(int)Material.light].bounce = 0.0f; + + TerrainFriction *= frictionScale; +// AvatarFriction *= frictionScale; + + // Set the gravity,, don't disable things automatically (we set it explicitly on some things) + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + + d.WorldSetLinearDamping(world, 0.001f); + d.WorldSetAngularDamping(world, 0.001f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 256f); + + d.WorldSetCFM(world,1e-6f); // a bit harder than default + //d.WorldSetCFM(world, 1e-4f); // a bit harder than default + d.WorldSetERP(world, 0.6f); // higher than original + + // Set how many steps we go without running collision testing + // This is in addition to the step size. + // Essentially Steps * m_physicsiterations + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + d.WorldSetContactMaxCorrectingVel(world, 100.0f); + + spacesPerMeter = 1 / metersInSpace; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // create all spaces now + int i, j; + IntPtr newspace; + for (i = 0; i < spaceGridMaxX; i++) + for (j = 0; j < spaceGridMaxY; j++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + staticPrimspace[i, j] = newspace; + } + // let this now be real maximum values + spaceGridMaxX--; + spaceGridMaxY--; + } + + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } + + #region Collision Detection + + // sets a global contact for a joint for contactgeom , and base contact description) + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp) + { + if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; + + d.Contact newcontact = new d.Contact(); + newcontact.geom.depth = contactGeom.depth; + newcontact.geom.g1 = contactGeom.g1; + newcontact.geom.g2 = contactGeom.g2; + newcontact.geom.pos = contactGeom.pos; + newcontact.geom.normal = contactGeom.normal; + newcontact.geom.side1 = contactGeom.side1; + newcontact.geom.side2 = contactGeom.side2; + + // this needs bounce also + newcontact.surface.mode = comumContactFlags; + newcontact.surface.mu = mu; + newcontact.surface.bounce = bounce; + newcontact.surface.soft_cfm = comumContactCFM; + if (softerp) + newcontact.surface.soft_erp = comumSoftContactERP; + else + newcontact.surface.soft_erp = comumContactERP; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(newcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); + } + + + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + /// + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + + + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + + if (m_global_contactcount >= maxContactsbeforedeath) + return; + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //here one should check collisions of geoms inside a space + // but on each space we only should have geoms that not colide amoung each other + // so we don't dig inside spaces + return; + } + + // get geom bodies to check if we already a joint contact + // guess this shouldn't happen now + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + + // d.GeomClassID id = d.GeomGetClass(g1); + + // Figure out how many contact points we have + int count = 0; + try + { + // Colliding Geom To Geom + // This portion of the function 'was' blatantly ripped off from BoxStack.cs + + if (g1 == g2) + return; // Can't collide with yourself + + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); +// ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + return; + } + + // id contacts done + if (count == 0) + return; + + // try get physical actors + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + p1 = PANull; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + p2 = PANull; + } + + // update actors collision score + if (p1.CollisionScore >= float.MaxValue - count) + p1.CollisionScore = 0; + p1.CollisionScore += count; + + if (p2.CollisionScore >= float.MaxValue - count) + p2.CollisionScore = 0; + p2.CollisionScore += count; + + + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) + return; + // for now it's the one with max depth + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth + ); + // do volume detection case + if ( + (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) || + (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + { + collision_accounting_events(p1, p2, maxDepthContact); + return; + } + + // big messy collision analises + float mu = 0; + float bounce = 0; + ContactData contactdata1; + ContactData contactdata2; + bool erpSoft = false; + + String name = null; + bool dop1foot = false; + bool dop2foot = false; + bool ignore = false; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + p1.CollidingObj = true; + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + dop1foot = true; + break; + default: + ignore=true; // avatar to terrain and water ignored + break; + } + break; + + case (int)ActorTypes.Prim: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + dop2foot = true; + if (p1.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + erpSoft = true; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + break; + default: + if (geom_name_map.TryGetValue(g2, out name)) + { + if (name == "Terrain") + { + erpSoft = true; + contactdata1 = p1.ContactData; + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + } + else if (name == "Water") + { + erpSoft = true; + } + } + else + ignore=true; + break; + } + break; + + default: + if (geom_name_map.TryGetValue(g1, out name)) + { + if (name == "Terrain") + { + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + erpSoft = true; + p2.CollidingGround = true; + contactdata2 = p2.ContactData; + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + } + else + ignore = true; + + } + else if (name == "Water" && + (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) + { + erpSoft = true; + } + } + else + ignore = true; + break; + } + + if (ignore) + return; + + IntPtr Joint; + + int i = 0; + while(true) + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + + Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft); + d.JointAttach(Joint, b1, b2); + + if (++m_global_contactcount >= maxContactsbeforedeath) + break; + + if(++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + } + } + + collision_accounting_events(p1, p2, maxDepthContact); + +/* + if (notskipedcount > geomContactPointsStartthrottle) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + this needs checking so out for now + if (b1 != IntPtr.Zero) + p1.ThrottleUpdates = true; + if (b2 != IntPtr.Zero) + p2.ThrottleUpdates = true; + + } + */ + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + // obj1LocalID = 0; + //returncollisions = false; + obj2LocalID = 0; + //ctype = 0; + //cStartStop = 0; + if (!(p2.SubscribedEvents() || p1.SubscribedEvents())) + return; + + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + cc1 = (OdeCharacter)p1; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cc1.m_localID, contact); + break; + + case ActorTypes.Prim: + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cc1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cc1.AddCollisionEvent(obj2LocalID, contact); + } + break; + + case ActorTypes.Prim: + + if (p1 is OdePrim) + { + cp1 = (OdePrim)p1; + + // obj1LocalID = cp2.m_localID; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + if (p2 is OdeCharacter) + { + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + case ActorTypes.Prim: + + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cp1.AddCollisionEvent(obj2LocalID, contact); + } + } + break; + } + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized() + { +// _perloopContact.Clear(); +// clear characts IsColliding until we do it some other way + + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + // this are odd checks if they are needed something is wrong elsewhere + // keep for now + if (chr == null) + continue; + + if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + } + } + + // now let ode do its job + // colide active things amoung them + + int st = Util.EnvironmentTickCount(); + int ta; + int ts; + try + { + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active space collide"); + } + ta = Util.EnvironmentTickCountSubtract(st); + // then active things with static enviroment + try + { + d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active to static space collide"); + } + ts = Util.EnvironmentTickCountSubtract(st); +// _perloopContact.Clear(); + } + + #endregion + + + public float GetTerrainHeightAtXY(float x, float y) + { + // assumes 1m size grid and constante size square regions + // region offset in mega position + + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; + + if (heightFieldGeom == IntPtr.Zero) + return 0f; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsize = (int)Constants.RegionSize + 2; // map size see setterrain + + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 1; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + + return h0 + h1 + h2; + } + + /// + /// Add actor to the list that should receive collision events in the simulate loop. + /// + /// + public void AddCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); + } + } + + /// + /// Remove actor from the list that should receive collision events in the simulate loop. + /// + /// + public void RemoveCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Remove(obj); + } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + Vector3 pos; + pos.X = position.X; + pos.Y = position.Y; + pos.Z = position.Z; + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + + return newAv; + } + + public void AddCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } + } + + public void RemoveCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } + } + } + + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) + { + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); + } + } + + public override void RemoveAvatar(PhysicsActor actor) + { + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, uint localID) + { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); + + lock (_prims) + _prims.Add(newPrim); + } + newPrim.LocalID = localID; + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. (ones that should be iterated over in collisions_optimized + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + //else + // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + { +#if SPAM + m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); +#endif + + return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } + + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { +// lock (OdeLock) + { + OdePrim p = (OdePrim)prim; + p.setPrimForRemoval(); + } + } + } + /// + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in + /// Simulate() -- justincc). + /// + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. + /// + /// + public void RemovePrimThreadLocked(OdePrim prim) + { + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { + RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim); + } + + } + #endregion + + #region Space Separation Calculation + + /// + /// Called when a static prim moves or becomes static + /// Places the prim in a space one the static sub-spaces grid + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // moves a prim into another static sub-space or from another space into a static sub-space + + // Called ODEPrim so + // it's already in locked space. + + if (geom == IntPtr.Zero) // shouldn't happen + return IntPtr.Zero; + + // get the static sub-space for current position + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == currentspace) // if we are there all done + return newspace; + + // else remove it from its current space + if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + + " Geom:" + geom); + } + } + else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space + { + currentspace = d.GeomGetSpace(geom); + if (currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + } + } + + // put the geom in the newspace + waitForSpaceUnlock(newspace); + d.SpaceAdd(newspace, geom); + + // let caller know this newspace + return newspace; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) + { + int x, y; + x = (int)(pos.X * spacesPerMeter); + if (x < 0) + x = 0; + else if (x > spaceGridMaxX) + x = spaceGridMaxX; + + y = (int)(pos.Y * spacesPerMeter); + if (y < 0) + y = 0; + else if (y >spaceGridMaxY) + y = spaceGridMaxY; + + IntPtr tmpSpace = staticPrimspace[x, y]; + return tmpSpace; + } + + #endregion + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // most of this is redundant now as the mesher will return null if it cant mesh a prim + // but we still need to check for sculptie meshing being enabled so this is the most + // convenient place to do it for now... + + // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) + // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); + int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry) + { + if(!meshSculptedPrim) + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + if (!forceSimplePrimMeshing && !pbs.SculptEntry) + { + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + } + } + + // following code doesn't give meshs to boxes and spheres ever + // and it's odd.. so for now just return true if asked to force meshs + // hopefully mesher will fail if doesn't suport so things still get basic boxes + + if (forceSimplePrimMeshing) + return true; + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (pbs.SculptEntry && meshSculptedPrim) + iPropertiesNotSupportedDefault++; + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + /// + /// Called to queue a change to a actor + /// to use in place of old taint mechanism so changes do have a time sequence + /// + + public void AddChange(PhysicsActor actor, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.actor = actor; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); + } + + /// + /// Called after our prim properties are set Scale, position etc. + /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex + /// This assures us that we have no race conditions + /// + /// + public override void AddPhysicsActorTaint(PhysicsActor prim) + { + } + + /// + /// This is our main simulate loop + /// It's thread locked by a Mutex in the scene. + /// It holds Collisions, it instructs ODE to step through the physical reactions + /// It moves the objects around in memory + /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) + /// + /// + /// + public override float Simulate(float timeStep) + { + int statstart; + int statchanges = 0; + int statchmove = 0; + int statactmove = 0; + int statray = 0; + int statcol = 0; + int statstep = 0; + int statmovchar = 0; + int statmovprim; + int totjcontact = 0; + + // acumulate time so we can reduce error + step_time += timeStep; + + if (step_time < ODE_STEPSIZE) + return 0; + + if (framecount >= int.MaxValue) + framecount = 0; + + framecount++; + + int curphysiteractions = m_physicsiterations; + + if (step_time >= m_SkipFramesAtms) + { + // if in trouble reduce step resolution + curphysiteractions /= 2; + } + + int nodeframes = 0; + +// checkThread(); + + lock (SimulationLock) + { + // adjust number of iterations per step + try + { + d.WorldSetQuickStepNumIterations(world, curphysiteractions); + } + catch (StackOverflowException) + { + m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); +// ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + + + while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever + { + try + { + statstart = Util.EnvironmentTickCount(); + + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; + + ODEchangeitem item; + + if(ChangesQueue.Count >0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; + int ttmp2; + + while(ChangesQueue.Dequeue(out item)) + { + if (item.actor != null) + { + try + { + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); + } + catch + { + m_log.Warn("[PHYSICS]: doChange failed for a actor"); + }; + } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + + ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp2 > 50) + ttmp2 = 0; + + } + + statchanges += Util.EnvironmentTickCountSubtract(statstart); + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + statchmove += Util.EnvironmentTickCountSubtract(statstart); + + // Move other active objects + lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + aprim.Move(); + } + } + + statactmove += Util.EnvironmentTickCountSubtract(statstart); + //if ((framecount % m_randomizeWater) == 0) + // randomizeWater(waterlevel); + + m_rayCastManager.ProcessQueuedRequests(); + + statray += Util.EnvironmentTickCountSubtract(statstart); + collision_optimized(); + statcol += Util.EnvironmentTickCountSubtract(statstart); + + lock (_collisionEventPrim) + { + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f)); + cobj.SendCollisions(); + break; + + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + pobj.SendCollisions(); + break; + } + } + } + + d.WorldQuickStep(world, ODE_STEPSIZE); + statstep += Util.EnvironmentTickCountSubtract(statstart); + d.JointGroupEmpty(contactgroup); + totjcontact += m_global_contactcount; + //ode.dunlock(world); + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + nodeframes++; + } + + statstart = Util.EnvironmentTickCount(); + + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + + _badCharacter.Clear(); + } + } + statmovchar = Util.EnvironmentTickCountSubtract(statstart); + + lock (_activeprims) + { + { + foreach (OdePrim actor in _activeprims) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE); + } + } + } + } + + statmovprim = Util.EnvironmentTickCountSubtract(statstart); + + int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; + + // Finished with all sim stepping. If requested, dump world state to file for debugging. + // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? + // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? + if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) + { + string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename + string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file + + if (physics_logging_append_existing_logfile) + { + string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; + TextWriter fwriter = File.AppendText(fname); + fwriter.WriteLine(header); + fwriter.Close(); + } + + d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); + } + + // think time dilation is not a physics issue alone.. but ok let's fake something + if (step_time < ODE_STEPSIZE) // we did the required loops + m_timeDilation = 1.0f; + else + { // we didn't forget the lost ones and let user know something + m_timeDilation = 1 - step_time / timeStep; + if (m_timeDilation < 0) + m_timeDilation = 0; + step_time = 0; + } + } + +// return nodeframes * ODE_STEPSIZE; // return real simulated time + return 1000 * nodeframes; // return steps for now * 1000 to keep core happy + } + + /// + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + #region ODE Specific Terrain Fixes + public float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; + } + } + + // Resize using Nearest Neighbour + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512, 512]; + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr2[y * 2, x * 2] = resultarr[y, x]; + + if (y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; + } + if (x < WorldExtents.X) + { + resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; + } + if (x < WorldExtents.X && y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; + } + } + } + + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (resultarr2[y, x] <= 0) + returnarr[i] = 0.0000001f; + else + returnarr[i] = resultarr2[y, x]; + + i++; + } + } + + return returnarr; + } + + public float[] ResizeTerrain512Interpolation(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[512,512]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < 256; y++) + { + for (int x = 0; x < 256; x++) + { + resultarr[y, x] = heightMap[y * 256 + x]; + } + } + + // Resize using interpolation + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512,512]; + for (int y = 0; y < (int)Constants.RegionSize; y++) + { + for (int x = 0; x < (int)Constants.RegionSize; x++) + { + resultarr2[y*2, x*2] = resultarr[y, x]; + + if (y < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); + } + } + else + { + resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); + } + } + else + { + resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) + { + if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) + { + resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; + } + } + } + } + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) + { + m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0"); + resultarr2[y, x] = 0; + } + returnarr[i] = resultarr2[y, x]; + i++; + } + } + + return returnarr; + } + + #endregion + + public override void SetTerrain(float[] heightMap) + { + if (m_worldOffset != Vector3.Zero && m_parentScene != null) + { + if (m_parentScene is OdeScene) + { + ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); + } + } + else + { + SetTerrain(heightMap, m_worldOffset); + } + } + + public override void CombineTerrain(float[] heightMap, Vector3 pOffset) + { + SetTerrain(heightMap, pOffset); + } + + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + + float[] _heightmap; + _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples; + + uint heightmapHeightSamples; + + heightmapWidthSamples = (uint)Constants.RegionSize + 2; + heightmapHeightSamples = (uint)Constants.RegionSize + 2; + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 10f; + const int wrap = 0; + + int regionsize = (int) Constants.RegionSize + 2; + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + int xx; + int yy; + + int maxXXYY = regionsize - 3; + // flipping map adding one margin all around so things don't fall in edges + + int xt = 0; + xx = 0; + + for (int x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && xx < maxXXYY) + xx++; + yy = 0; + for (int y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += (int)Constants.RegionSize; + + val = heightMap[yy + xx]; + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; + + } + + xt += regionsize; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(StaticSpace, GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0); + IntPtr testGround = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out testGround)) + { + RegionTerrain.Remove(pOffset); + } + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); +// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override bool SupportsCombining() + { + return true; + } +/* + public override void UnCombine(PhysicsScene pScene) + { + IntPtr localGround = IntPtr.Zero; +// float[] localHeightfield; + bool proceed = false; + List geomDestroyList = new List(); + + lock (OdeLock) + { + if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) + { + foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) + { + if (geom == localGround) + { +// localHeightfield = TerrainHeightFieldHeights[geom]; + proceed = true; + } + else + { + geomDestroyList.Add(geom); + } + } + + if (proceed) + { + m_worldOffset = Vector3.Zero; + WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + m_parentScene = null; + + foreach (IntPtr g in geomDestroyList) + { + // removingHeightField needs to be done or the garbage collector will + // collect the terrain data before we tell ODE to destroy it causing + // memory corruption + if (TerrainHeightFieldHeights.ContainsKey(g)) + { +// float[] removingHeightField = TerrainHeightFieldHeights[g]; + TerrainHeightFieldHeights.Remove(g); + + if (RegionTerrain.ContainsKey(g)) + { + RegionTerrain.Remove(g); + } + + d.GeomDestroy(g); + //removingHeightField = new float[0]; + } + } + + } + else + { + m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + } + } + } + } +*/ + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + randomizeWater(waterlevel); + } + + public void randomizeWater(float baseheight) + { + const uint heightmapWidth = m_regionWidth + 2; + const uint heightmapHeight = m_regionHeight + 2; + const uint heightmapWidthSamples = m_regionWidth + 2; + const uint heightmapHeightSamples = m_regionHeight + 2; + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 2.9f; + const int wrap = 0; + + for (int i = 0; i < (258 * 258); i++) + { + _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + } + + lock (OdeLock) + { + if (WaterGeom != IntPtr.Zero) + { + d.SpaceRemove(StaticSpace, WaterGeom); + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); + WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (WaterGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[WaterGeom] = "Water"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + q1 = q1 * q2; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, 128, 128, 0); + + } + + } + + public override void Dispose() + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + + lock (OdeLock) + { + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + RemovePrim(prm); + } + } + + if (ContactgeomsArray != IntPtr.Zero) + Marshal.FreeHGlobal(ContactgeomsArray); + if (GlobalContactsArray != IntPtr.Zero) + Marshal.FreeHGlobal(GlobalContactsArray); + + d.WorldDestroy(world); + //d.CloseODE(); + } + } + + public override Dictionary GetTopColliders() + { + Dictionary returncolliders = new Dictionary(); + int cnt = 0; + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.m_localID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } + } + return returncolliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, retMethod); + } + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) + { + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null && actor !=null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); + } + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null && actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + if (actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + if (geom == IntPtr.Zero) + return new List(); + + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + return new List(); + } + } +} diff --git a/bin/Physics/OpenSim.Region.Physics.UbitOdePlugin.dll.config b/bin/Physics/OpenSim.Region.Physics.UbitOdePlugin.dll.config new file mode 100644 index 0000000000..f8f071e8c6 --- /dev/null +++ b/bin/Physics/OpenSim.Region.Physics.UbitOdePlugin.dll.config @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/bin/Physics/libode-x86_64.so b/bin/Physics/libode-x86_64.so new file mode 100644 index 0000000000..9c3070abf4 Binary files /dev/null and b/bin/Physics/libode-x86_64.so differ diff --git a/bin/Physics/libode.so b/bin/Physics/libode.so new file mode 100644 index 0000000000..6bb85fb72a Binary files /dev/null and b/bin/Physics/libode.so differ diff --git a/prebuild.xml b/prebuild.xml index 77cae6eef3..443c4f809f 100644 --- a/prebuild.xml +++ b/prebuild.xml @@ -6,7 +6,7 @@ TRACE;DEBUG false false - false + true 4 false @@ -606,6 +606,36 @@ + + + + ../../../../bin/Physics/ + + + + + ../../../../bin/Physics/ + + + + ../../../../bin/ + + + + + + + + + + + + + + + + + @@ -3316,16 +3346,12 @@ - - - - - + diff --git a/runprebuild.bat b/runprebuild.bat index 8927e9a7ee..9265391917 100755 --- a/runprebuild.bat +++ b/runprebuild.bat @@ -1,4 +1,4 @@ bin\Prebuild.exe /target nant bin\Prebuild.exe /target vs2008 -echo C:\WINDOWS\Microsoft.NET\Framework\v3.5\msbuild opensim.sln > compile.bat +echo C:\WINDOWS\Microsoft.NET\Framework\v3.5\msbuild OpenSim.sln > compile.bat