From 5e4d6cab00cb29cd088ab7b62ab13aff103b64cb Mon Sep 17 00:00:00 2001 From: onefang Date: Sun, 19 May 2019 21:24:15 +1000 Subject: Dump OpenSim 0.9.0.1 into it's own branch. --- .../PhysicsModules/BasicPhysics/AssemblyInfo.cs | 2 +- .../BasicPhysics/BasicPhysicsActor.cs | 19 +- .../BasicPhysics/BasicPhysicsPrim.cs | 19 +- .../BasicPhysics/BasicPhysicsScene.cs | 8 +- .../Region/PhysicsModules/BulletS/BSAPIUnman.cs | 30 +- OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs | 318 +- .../PhysicsModules/BulletS/BSActorAvatarMove.cs | 106 +- .../Region/PhysicsModules/BulletS/BSActorHover.cs | 0 .../PhysicsModules/BulletS/BSActorLockAxis.cs | 0 .../PhysicsModules/BulletS/BSActorMoveToTarget.cs | 4 +- .../PhysicsModules/BulletS/BSActorSetForce.cs | 0 .../PhysicsModules/BulletS/BSActorSetTorque.cs | 2 +- OpenSim/Region/PhysicsModules/BulletS/BSActors.cs | 0 .../Region/PhysicsModules/BulletS/BSApiTemplate.cs | 141 +- .../Region/PhysicsModules/BulletS/BSCharacter.cs | 323 +- .../Region/PhysicsModules/BulletS/BSConstraint.cs | 0 .../PhysicsModules/BulletS/BSConstraint6Dof.cs | 0 .../BulletS/BSConstraintCollection.cs | 0 .../BulletS/BSConstraintConeTwist.cs | 0 .../PhysicsModules/BulletS/BSConstraintHinge.cs | 0 .../PhysicsModules/BulletS/BSConstraintSlider.cs | 0 .../PhysicsModules/BulletS/BSConstraintSpring.cs | 0 .../Region/PhysicsModules/BulletS/BSDynamics.cs | 10 +- OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs | 4 +- .../PhysicsModules/BulletS/BSLinksetCompound.cs | 1 + .../PhysicsModules/BulletS/BSLinksetConstraints.cs | 0 .../Region/PhysicsModules/BulletS/BSMaterials.cs | 0 OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs | 0 OpenSim/Region/PhysicsModules/BulletS/BSParam.cs | 238 +- .../Region/PhysicsModules/BulletS/BSPhysObject.cs | 123 +- OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs | 157 +- .../PhysicsModules/BulletS/BSPrimDisplaced.cs | 1 - .../PhysicsModules/BulletS/BSPrimLinkable.cs | 7 +- OpenSim/Region/PhysicsModules/BulletS/BSScene.cs | 329 +- .../PhysicsModules/BulletS/BSShapeCollection.cs | 7 +- OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs | 10 +- .../PhysicsModules/BulletS/BSTerrainHeightmap.cs | 24 +- .../PhysicsModules/BulletS/BSTerrainManager.cs | 0 .../Region/PhysicsModules/BulletS/BSTerrainMesh.cs | 0 .../Region/PhysicsModules/BulletS/BulletSimData.cs | 1 + .../PhysicsModules/BulletS/BulletSimTODO.txt | 0 .../PhysicsModules/BulletS/ExtendedPhysics.cs | 2 +- .../BulletS/Properties/AssemblyInfo.cs | 10 +- .../PhysicsModules/BulletS/Tests/BasicVehicles.cs | 0 .../PhysicsModules/BulletS/Tests/BulletSimTests.cs | 112 +- .../BulletS/Tests/BulletSimTestsUtil.cs | 6 +- .../PhysicsModules/BulletS/Tests/HullCreation.cs | 2 +- .../Region/PhysicsModules/BulletS/Tests/Raycast.cs | 124 + .../ConvexDecompositionDotNet/CTri.cs | 26 +- .../ConvexDecompositionDotNet/Concavity.cs | 12 +- .../ConvexDecompositionDotNet/ConvexBuilder.cs | 12 +- .../ConvexDecomposition.cs | 12 +- .../ConvexDecompositionDotNet/ConvexResult.cs | 12 +- .../ConvexDecompositionDotNet/HullClasses.cs | 12 +- .../ConvexDecompositionDotNet/HullTriangle.cs | 12 +- .../ConvexDecompositionDotNet/HullUtils.cs | 79 +- .../ConvexDecompositionDotNet/Plane.cs | 12 +- .../ConvexDecompositionDotNet/PlaneTri.cs | 12 +- .../Properties/AssemblyInfo.cs | 12 +- .../ConvexDecompositionDotNet/Quaternion.cs | 18 +- .../ConvexDecompositionDotNet/SplitPlane.cs | 12 +- .../ConvexDecompositionDotNet/VertexLookup.cs | 12 +- .../ConvexDecompositionDotNet/float2.cs | 12 +- .../ConvexDecompositionDotNet/float3.cs | 12 +- .../ConvexDecompositionDotNet/float3x3.cs | 12 +- .../ConvexDecompositionDotNet/float4.cs | 12 +- .../ConvexDecompositionDotNet/float4x4.cs | 170 +- .../ConvexDecompositionDotNet/int3.cs | 12 +- .../ConvexDecompositionDotNet/int4.cs | 12 +- .../Meshing/Meshmerizer/HelperTypes.cs | 2 +- .../PhysicsModules/Meshing/Meshmerizer/Mesh.cs | 87 +- .../Meshing/Meshmerizer/Meshmerizer.cs | 43 +- .../Meshing/Meshmerizer/PrimMesher.cs | 6 +- .../Meshing/Meshmerizer/SculptMap.cs | 60 +- .../Meshing/Properties/AssemblyInfo.cs | 14 +- .../Region/PhysicsModules/Meshing/ZeroMesher.cs | 25 +- OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs | 2 +- OpenSim/Region/PhysicsModules/Ode/ODEApi.cs | 2025 ++++++++++ OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs | 33 +- OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs | 11 +- OpenSim/Region/PhysicsModules/Ode/ODEModule.cs | 89 + OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | 720 ++-- .../PhysicsModules/Ode/ODERayCastRequestManager.cs | 97 +- .../Region/PhysicsModules/Ode/OdePhysicsJoint.cs | 2 - OpenSim/Region/PhysicsModules/Ode/OdeScene.cs | 980 +---- .../PhysicsModules/Ode/Tests/ODETestClass.cs | 18 +- OpenSim/Region/PhysicsModules/Ode/drawstuff.cs | 98 - OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs | 2 +- OpenSim/Region/PhysicsModules/POS/POSCharacter.cs | 11 +- OpenSim/Region/PhysicsModules/POS/POSPrim.cs | 5 +- OpenSim/Region/PhysicsModules/POS/POSScene.cs | 8 +- .../PhysicsModules/SharedBase/AssemblyInfo.cs | 2 +- .../Region/PhysicsModules/SharedBase/IMesher.cs | 31 +- .../SharedBase/IPhysicsParameters.cs | 0 .../PhysicsModules/SharedBase/NullPhysicsScene.cs | 2 +- .../PhysicsModules/SharedBase/PhysicsActor.cs | 311 +- .../PhysicsModules/SharedBase/PhysicsScene.cs | 85 +- .../PhysicsModules/SharedBase/PhysicsVector.cs | 2 +- .../PhysicsModules/SharedBase/VehicleConstants.cs | 47 +- OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs | 2024 ++++++++++ .../Region/PhysicsModules/ubOde/ODECharacter.cs | 2036 ++++++++++ OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs | 1205 ++++++ .../Region/PhysicsModules/ubOde/ODEMeshWorker.cs | 946 +++++ OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs | 112 + OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs | 4256 ++++++++++++++++++++ .../ubOde/ODERayCastRequestManager.cs | 689 ++++ OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs | 2824 +++++++++++++ .../Region/PhysicsModules/ubOde/ODESitAvatar.cs | 356 ++ .../ubOde/Properties/AssemblyInfo.cs | 61 + .../PhysicsModules/ubOdeMeshing/HelperTypes.cs | 340 ++ OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs | 636 +++ .../PhysicsModules/ubOdeMeshing/Meshmerizer.cs | 1602 ++++++++ .../PhysicsModules/ubOdeMeshing/PrimMesher.cs | 1707 ++++++++ .../ubOdeMeshing/Properties/AssemblyInfo.cs | 36 + .../PhysicsModules/ubOdeMeshing/SculptMap.cs | 238 ++ .../PhysicsModules/ubOdeMeshing/SculptMesh.cs | 220 + 116 files changed, 24079 insertions(+), 2664 deletions(-) mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSActors.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSParam.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs create mode 100755 OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs create mode 100644 OpenSim/Region/PhysicsModules/Ode/ODEApi.cs create mode 100644 OpenSim/Region/PhysicsModules/Ode/ODEModule.cs delete mode 100644 OpenSim/Region/PhysicsModules/Ode/drawstuff.cs mode change 100644 => 100755 OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs create mode 100644 OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs (limited to 'OpenSim/Region/PhysicsModules') diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs index 1765ae0..68d6eb9 100644 --- a/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/BasicPhysics/AssemblyInfo.cs @@ -56,7 +56,7 @@ using Mono.Addins; // 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.8.2.*")] +[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] [assembly: Addin("OpenSim.Region.PhysicsModule.BasicPhysics", OpenSim.VersionInfo.VersionNumber)] [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs index e7b30ba..67d9207 100644 --- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs +++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsActor.cs @@ -211,7 +211,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics { } - public override void LockAngularMotion(Vector3 axis) + public override void LockAngularMotion(byte axislocks) { } @@ -254,6 +254,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics public override bool PIDHoverActive { + get { return false; } set { return; } } @@ -267,23 +268,23 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics set { return; } } - public override Quaternion APIDTarget - { + public override Quaternion APIDTarget + { set { return; } } - + public override bool APIDActive - { + { set { return; } } - + public override float APIDStrength - { + { set { return; } } - + public override float APIDDamping - { + { set { return; } } diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs index 5383f1b..58c2581 100644 --- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs +++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsPrim.cs @@ -224,7 +224,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics { } - public override void LockAngularMotion(Vector3 axis) + public override void LockAngularMotion(byte axislocks) { } @@ -267,6 +267,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics public override bool PIDHoverActive { + get { return false; } set { return; } } @@ -280,23 +281,23 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics set { return; } } - public override Quaternion APIDTarget - { + public override Quaternion APIDTarget + { set { return; } } - + public override bool APIDActive - { + { set { return; } } - + public override float APIDStrength - { + { set { return; } } - + public override float APIDDamping - { + { set { return; } } diff --git a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs index 20b337a..2a1ae45 100644 --- a/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs +++ b/OpenSim/Region/PhysicsModules/BasicPhysics/BasicPhysicsScene.cs @@ -61,6 +61,11 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics get { return "basicphysics"; } } + public string Version + { + get { return "1.0"; } + } + public Type ReplaceableInterface { get { return null; } @@ -90,6 +95,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics EngineType = Name; PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName; + EngineName = Name + " " + Version; scene.RegisterModuleInterface(this); m_regionExtent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ); @@ -220,7 +226,7 @@ namespace OpenSim.Region.PhysicsModule.BasicPhysics actor.Velocity = actorVelocity; } - return fps; + return 1.0f; } public override void GetResults() diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs old mode 100644 new mode 100755 index c4a923c..840e453 --- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIUnman.cs @@ -155,8 +155,8 @@ public BSAPIUnman(string paramName, BSScene physScene) // Initialization and simulation public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms, - int maxCollisions, ref CollisionDesc[] collisionArray, - int maxUpdates, ref EntityProperties[] updateArray + int maxCollisions, ref CollisionDesc[] collisionArray, + int maxUpdates, ref EntityProperties[] updateArray ) { // Pin down the memory that will be used to pass object collisions and updates back from unmanaged code @@ -1405,6 +1405,19 @@ public override float GetMargin(BulletShape shape) } // ===================================================================================== +// Raycast +public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody sweepObject, Vector3 from, Vector3 to, float margin) { + BulletWorldUnman worldu = world as BulletWorldUnman; + BulletBodyUnman bodyu = sweepObject as BulletBodyUnman; + return BSAPICPP.ConvexSweepTest2(worldu.ptr, bodyu.ptr, from, to, margin); +} + +public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) { + BulletWorldUnman worldu = world as BulletWorldUnman; + return BSAPICPP.RayTest2(worldu.ptr, from, to, filterGroup, filterMask); +} + +// ===================================================================================== // Debugging public override void DumpRigidBody(BulletWorld world, BulletBody collisionObject) { @@ -1472,8 +1485,8 @@ public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg // Initialization and simulation [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] public static extern IntPtr Initialize2(Vector3 maxPosition, IntPtr parms, - int maxCollisions, IntPtr collisionArray, - int maxUpdates, IntPtr updateArray, + int maxCollisions, IntPtr collisionArray, + int maxUpdates, IntPtr updateArray, DebugLogCallback logRoutine); [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] @@ -2084,6 +2097,15 @@ public static extern void SetMargin2(IntPtr shape, float val); [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] public static extern float GetMargin2(IntPtr shape); + +// ===================================================================================== +// Raycast +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern SweepHit ConvexSweepTest2(IntPtr sim, IntPtr obj, Vector3 from, Vector3 to, float margin); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern RaycastHit RayTest2(IntPtr sim, Vector3 from, Vector3 to, uint filterGroup, uint filterMask); + // ===================================================================================== // Debugging [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs old mode 100644 new mode 100755 index 887311d..7d58728 --- a/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSAPIXNA.cs @@ -815,161 +815,161 @@ private sealed class BulletConstraintXNA : BulletConstraint public override bool SliderSetLimits(BulletConstraint pConstraint, int lowerUpper, int linAng, float val) { SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint; - switch (lowerUpper) - { - case SLIDER_LOWER_LIMIT: - switch (linAng) - { - case SLIDER_LINEAR: - constraint.SetLowerLinLimit(val); - break; - case SLIDER_ANGULAR: - constraint.SetLowerAngLimit(val); - break; - } - break; - case SLIDER_UPPER_LIMIT: - switch (linAng) - { - case SLIDER_LINEAR: - constraint.SetUpperLinLimit(val); - break; - case SLIDER_ANGULAR: - constraint.SetUpperAngLimit(val); - break; - } - break; - } + switch (lowerUpper) + { + case SLIDER_LOWER_LIMIT: + switch (linAng) + { + case SLIDER_LINEAR: + constraint.SetLowerLinLimit(val); + break; + case SLIDER_ANGULAR: + constraint.SetLowerAngLimit(val); + break; + } + break; + case SLIDER_UPPER_LIMIT: + switch (linAng) + { + case SLIDER_LINEAR: + constraint.SetUpperLinLimit(val); + break; + case SLIDER_ANGULAR: + constraint.SetUpperAngLimit(val); + break; + } + break; + } return true; } public override bool SliderSet(BulletConstraint pConstraint, int softRestDamp, int dirLimOrtho, int linAng, float val) { SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint; - switch (softRestDamp) - { - case SLIDER_SET_SOFTNESS: - switch (dirLimOrtho) - { - case SLIDER_SET_DIRECTION: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetSoftnessDirLin(val); break; - case SLIDER_ANGULAR: constraint.SetSoftnessDirAng(val); break; - } - break; - case SLIDER_SET_LIMIT: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetSoftnessLimLin(val); break; - case SLIDER_ANGULAR: constraint.SetSoftnessLimAng(val); break; - } - break; - case SLIDER_SET_ORTHO: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetSoftnessOrthoLin(val); break; - case SLIDER_ANGULAR: constraint.SetSoftnessOrthoAng(val); break; - } - break; - } - break; - case SLIDER_SET_RESTITUTION: - switch (dirLimOrtho) - { - case SLIDER_SET_DIRECTION: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetRestitutionDirLin(val); break; - case SLIDER_ANGULAR: constraint.SetRestitutionDirAng(val); break; - } - break; - case SLIDER_SET_LIMIT: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetRestitutionLimLin(val); break; - case SLIDER_ANGULAR: constraint.SetRestitutionLimAng(val); break; - } - break; - case SLIDER_SET_ORTHO: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetRestitutionOrthoLin(val); break; - case SLIDER_ANGULAR: constraint.SetRestitutionOrthoAng(val); break; - } - break; - } - break; - case SLIDER_SET_DAMPING: - switch (dirLimOrtho) - { - case SLIDER_SET_DIRECTION: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetDampingDirLin(val); break; - case SLIDER_ANGULAR: constraint.SetDampingDirAng(val); break; - } - break; - case SLIDER_SET_LIMIT: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetDampingLimLin(val); break; - case SLIDER_ANGULAR: constraint.SetDampingLimAng(val); break; - } - break; - case SLIDER_SET_ORTHO: - switch (linAng) - { - case SLIDER_LINEAR: constraint.SetDampingOrthoLin(val); break; - case SLIDER_ANGULAR: constraint.SetDampingOrthoAng(val); break; - } - break; - } - break; - } + switch (softRestDamp) + { + case SLIDER_SET_SOFTNESS: + switch (dirLimOrtho) + { + case SLIDER_SET_DIRECTION: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetSoftnessDirLin(val); break; + case SLIDER_ANGULAR: constraint.SetSoftnessDirAng(val); break; + } + break; + case SLIDER_SET_LIMIT: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetSoftnessLimLin(val); break; + case SLIDER_ANGULAR: constraint.SetSoftnessLimAng(val); break; + } + break; + case SLIDER_SET_ORTHO: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetSoftnessOrthoLin(val); break; + case SLIDER_ANGULAR: constraint.SetSoftnessOrthoAng(val); break; + } + break; + } + break; + case SLIDER_SET_RESTITUTION: + switch (dirLimOrtho) + { + case SLIDER_SET_DIRECTION: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetRestitutionDirLin(val); break; + case SLIDER_ANGULAR: constraint.SetRestitutionDirAng(val); break; + } + break; + case SLIDER_SET_LIMIT: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetRestitutionLimLin(val); break; + case SLIDER_ANGULAR: constraint.SetRestitutionLimAng(val); break; + } + break; + case SLIDER_SET_ORTHO: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetRestitutionOrthoLin(val); break; + case SLIDER_ANGULAR: constraint.SetRestitutionOrthoAng(val); break; + } + break; + } + break; + case SLIDER_SET_DAMPING: + switch (dirLimOrtho) + { + case SLIDER_SET_DIRECTION: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetDampingDirLin(val); break; + case SLIDER_ANGULAR: constraint.SetDampingDirAng(val); break; + } + break; + case SLIDER_SET_LIMIT: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetDampingLimLin(val); break; + case SLIDER_ANGULAR: constraint.SetDampingLimAng(val); break; + } + break; + case SLIDER_SET_ORTHO: + switch (linAng) + { + case SLIDER_LINEAR: constraint.SetDampingOrthoLin(val); break; + case SLIDER_ANGULAR: constraint.SetDampingOrthoAng(val); break; + } + break; + } + break; + } return true; } public override bool SliderMotorEnable(BulletConstraint pConstraint, int linAng, float numericTrueFalse) { SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint; - switch (linAng) - { - case SLIDER_LINEAR: - constraint.SetPoweredLinMotor(numericTrueFalse == 0.0 ? false : true); - break; - case SLIDER_ANGULAR: - constraint.SetPoweredAngMotor(numericTrueFalse == 0.0 ? false : true); - break; - } + switch (linAng) + { + case SLIDER_LINEAR: + constraint.SetPoweredLinMotor(numericTrueFalse == 0.0 ? false : true); + break; + case SLIDER_ANGULAR: + constraint.SetPoweredAngMotor(numericTrueFalse == 0.0 ? false : true); + break; + } return true; } public override bool SliderMotor(BulletConstraint pConstraint, int forceVel, int linAng, float val) { SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint; - switch (forceVel) - { - case SLIDER_MOTOR_VELOCITY: - switch (linAng) - { - case SLIDER_LINEAR: - constraint.SetTargetLinMotorVelocity(val); - break; - case SLIDER_ANGULAR: - constraint.SetTargetAngMotorVelocity(val); - break; - } - break; - case SLIDER_MAX_MOTOR_FORCE: - switch (linAng) - { - case SLIDER_LINEAR: - constraint.SetMaxLinMotorForce(val); - break; - case SLIDER_ANGULAR: - constraint.SetMaxAngMotorForce(val); - break; - } - break; - } + switch (forceVel) + { + case SLIDER_MOTOR_VELOCITY: + switch (linAng) + { + case SLIDER_LINEAR: + constraint.SetTargetLinMotorVelocity(val); + break; + case SLIDER_ANGULAR: + constraint.SetTargetAngMotorVelocity(val); + break; + } + break; + case SLIDER_MAX_MOTOR_FORCE: + switch (linAng) + { + case SLIDER_LINEAR: + constraint.SetMaxLinMotorForce(val); + break; + case SLIDER_ANGULAR: + constraint.SetMaxAngMotorForce(val); + break; + } + break; + } return true; } @@ -1197,20 +1197,20 @@ private sealed class BulletConstraintXNA : BulletConstraint }; /* m_mass = mass; - m_motionState =motionState; - m_collisionShape = collisionShape; - m_localInertia = localInertia; - m_linearDamping = 0f; - m_angularDamping = 0f; - m_friction = 0.5f; - m_restitution = 0f; - m_linearSleepingThreshold = 0.8f; - m_angularSleepingThreshold = 1f; - m_additionalDamping = false; - m_additionalDampingFactor = 0.005f; - m_additionalLinearDampingThresholdSqr = 0.01f; - m_additionalAngularDampingThresholdSqr = 0.01f; - m_additionalAngularDampingFactor = 0.01f; + m_motionState =motionState; + m_collisionShape = collisionShape; + m_localInertia = localInertia; + m_linearDamping = 0f; + m_angularDamping = 0f; + m_friction = 0.5f; + m_restitution = 0f; + m_linearSleepingThreshold = 0.8f; + m_angularSleepingThreshold = 1f; + m_additionalDamping = false; + m_additionalDampingFactor = 0.005f; + m_additionalLinearDampingThresholdSqr = 0.01f; + m_additionalAngularDampingThresholdSqr = 0.01f; + m_additionalAngularDampingFactor = 0.01f; m_startWorldTransform = IndexedMatrix.Identity; */ body.SetUserPointer(pLocalID); @@ -2172,7 +2172,7 @@ private sealed class BulletConstraintXNA : BulletConstraint } public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, - float scaleFactor, float collisionMargin) + float scaleFactor, float collisionMargin) { const int upAxis = 2; HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y, @@ -2459,6 +2459,14 @@ private sealed class BulletConstraintXNA : BulletConstraint } return false; } + + public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin) { + return new SweepHit(); + } + + public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) { + return new RaycastHit(); + } } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs old mode 100644 new mode 100755 index 0191893..40c6b98 --- a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs @@ -47,10 +47,9 @@ public class BSActorAvatarMove : BSActor // The amount the step up is applying. Used to smooth stair walking. float m_lastStepUp; - // Jumping happens over several frames. If use applies up force while colliding, start the - // jump and allow the jump to continue for this number of frames. - int m_jumpFrames = 0; - float m_jumpVelocity = 0f; + // There are times the velocity is set but we don't want to inforce stationary until the + // real velocity drops. + bool m_waitingForLowVelocityForStationary = false; public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName) : base(physicsScene, pObj, actorName) @@ -109,19 +108,23 @@ public class BSActorAvatarMove : BSActor { if (m_velocityMotor != null) { -// if (targ == OMV.Vector3.Zero) -// Util.PrintCallStack(); -// -// Console.WriteLine("SetVelocityAndTarget, {0} {1}", vel, targ); m_velocityMotor.Reset(); m_velocityMotor.SetTarget(targ); m_velocityMotor.SetCurrent(vel); m_velocityMotor.Enabled = true; + m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,SetVelocityAndTarget,vel={1}, targ={2}", + m_controllingPrim.LocalID, vel, targ); + m_waitingForLowVelocityForStationary = false; } }); } - // If a hover motor has not been created, create one and start the hovering. + public void SuppressStationayCheckUntilLowVelocity() + { + m_waitingForLowVelocityForStationary = true; + } + + // If a movement motor has not been created, create one and start the movement private void ActivateAvatarMove() { if (m_velocityMotor == null) @@ -133,13 +136,14 @@ public class BSActorAvatarMove : BSActor 1f // efficiency ); m_velocityMotor.ErrorZeroThreshold = BSParam.AvatarStopZeroThreshold; - // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages. + // m_velocityMotor.PhysicsScene = m_controllingPrim.PhysScene; // DEBUG DEBUG so motor will output detail log messages. SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */); m_physicsScene.BeforeStep += Mover; m_controllingPrim.OnPreUpdateProperty += Process_OnPreUpdateProperty; m_walkingUpStairs = 0; + m_waitingForLowVelocityForStationary = false; } } @@ -153,7 +157,7 @@ public class BSActorAvatarMove : BSActor } } - // Called just before the simulation step. Update the vertical position for hoverness. + // Called just before the simulation step. private void Mover(float timeStep) { // Don't do movement while the object is selected. @@ -187,12 +191,28 @@ public class BSActorAvatarMove : BSActor if (m_controllingPrim.IsColliding) { - // If we are colliding with a stationary object, presume we're standing and don't move around + // if colliding with something stationary and we're not doing volume detect . if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.ColliderIsVolumeDetect) { - m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID); - m_controllingPrim.IsStationary = true; - m_controllingPrim.ZeroMotion(true /* inTaintTime */); + if (m_waitingForLowVelocityForStationary) + { + // if waiting for velocity to drop and it has finally dropped, we can be stationary + if (m_controllingPrim.RawVelocity.LengthSquared() < BSParam.AvatarStopZeroThresholdSquared) + { + m_waitingForLowVelocityForStationary = false; + } + } + if (!m_waitingForLowVelocityForStationary) + { + m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID); + m_controllingPrim.IsStationary = true; + m_controllingPrim.ZeroMotion(true /* inTaintTime */); + } + else + { + m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,waitingForLowVel,rawvel={1}", + m_controllingPrim.LocalID, m_controllingPrim.RawVelocity.Length()); + } } // Standing has more friction on the ground @@ -222,8 +242,8 @@ public class BSActorAvatarMove : BSActor } } - m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}", - m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding); + m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2},isStationary={3}", + m_controllingPrim.LocalID, m_velocityMotor.TargetValue, m_controllingPrim.IsColliding,m_controllingPrim.IsStationary); } else { @@ -237,50 +257,24 @@ public class BSActorAvatarMove : BSActor m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction); } - if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding) - { - stepVelocity.Z = m_controllingPrim.RawVelocity.Z; - } + // 'm_velocityMotor is used for walking, flying, and jumping and will thus have the correct values + // for Z. But in come cases it must be over-ridden. Like when falling or jumping. - // Colliding and not flying with an upward force. The avatar must be trying to jump. - if (!m_controllingPrim.Flying && m_controllingPrim.IsColliding && stepVelocity.Z > 0) - { - // We allow the upward force to happen for this many frames. - m_jumpFrames = BSParam.AvatarJumpFrames; - m_jumpVelocity = stepVelocity.Z; - } + float realVelocityZ = m_controllingPrim.RawVelocity.Z; - // The case where the avatar is not colliding and is not flying is special. - // The avatar is either falling or jumping and the user can be applying force to the avatar - // (force in some direction or force up or down). - // If the avatar has negative Z velocity and is not colliding, presume we're falling and keep the velocity. - // If the user is trying to apply upward force but we're not colliding, assume the avatar - // is trying to jump and don't apply the upward force if not touching the ground any more. - if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding) + // If not flying and falling, we over-ride the stepping motor so we can fall to the ground + if (!m_controllingPrim.Flying && realVelocityZ < 0) { - // If upward velocity is being applied, this must be a jump and only allow that to go on so long - if (m_jumpFrames > 0) + // Can't fall faster than this + if (realVelocityZ < BSParam.AvatarTerminalVelocity) { - // Since not touching the ground, only apply upward force for so long. - m_jumpFrames--; - stepVelocity.Z = m_jumpVelocity; + realVelocityZ = BSParam.AvatarTerminalVelocity; } - else - { - - // Since we're not affected by anything, the avatar must be falling and we do not want that to be too fast. - if (m_controllingPrim.RawVelocity.Z < BSParam.AvatarTerminalVelocity) - { - - stepVelocity.Z = BSParam.AvatarTerminalVelocity; - } - else - { - stepVelocity.Z = m_controllingPrim.RawVelocity.Z; - } - } - // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity); + + stepVelocity.Z = realVelocityZ; } + // m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,DEBUG,motorCurrent={1},realZ={2},flying={3},collid={4},jFrames={5}", + // m_controllingPrim.LocalID, m_velocityMotor.CurrentValue, realVelocityZ, m_controllingPrim.Flying, m_controllingPrim.IsColliding, m_jumpFrames); //Alicia: Maintain minimum height when flying. // SL has a flying effect that keeps the avatar flying above the ground by some margin @@ -291,6 +285,8 @@ public class BSActorAvatarMove : BSActor if( m_controllingPrim.Position.Z < hover_height) { + m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,addingUpforceForGroundMargin,height={1},hoverHeight={2}", + m_controllingPrim.LocalID, m_controllingPrim.Position.Z, hover_height); stepVelocity.Z += BSParam.AvatarFlyingGroundUpForce; } } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorHover.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorLockAxis.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs old mode 100644 new mode 100755 index 3db8f2c..f2019ae --- a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs @@ -205,13 +205,13 @@ public class BSActorMoveToTarget : BSActor addedForce = correctionVector / timeStep; // Remove the existing velocity (only the moveToTarget force counts) addedForce -= m_controllingPrim.RawVelocity; - // Overcome gravity. + // Overcome gravity. addedForce -= m_controllingPrim.Gravity; // Add enough force to overcome the mass of the object addedForce *= m_controllingPrim.Mass; - m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */); + m_controllingPrim.AddForce(true /* inTaintTime */, addedForce); } m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}", m_controllingPrim.LocalID, origPosition, addedForce); diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetForce.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs old mode 100644 new mode 100755 index a1cf4db..0261bcb --- a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs @@ -127,7 +127,7 @@ public class BSActorSetTorque : BSActor m_physicsScene.DetailLog("{0},BSActorSetTorque,preStep,force={1}", m_controllingPrim.LocalID, m_controllingPrim.RawTorque); if (m_controllingPrim.PhysBody.HasPhysicalBody) { - m_controllingPrim.AddAngularForce(m_controllingPrim.RawTorque, false, true); + m_controllingPrim.AddAngularForce(true /* inTaintTime */, m_controllingPrim.RawTorque); m_controllingPrim.ActivateIfPhysical(false); } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActors.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs index 7756b10..a288048 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSApiTemplate.cs @@ -36,16 +36,16 @@ namespace OpenSim.Region.PhysicsModule.BulletS { // Constraint type values as defined by Bullet public enum ConstraintType : int { - POINT2POINT_CONSTRAINT_TYPE = 3, - HINGE_CONSTRAINT_TYPE, - CONETWIST_CONSTRAINT_TYPE, - D6_CONSTRAINT_TYPE, - SLIDER_CONSTRAINT_TYPE, - CONTACT_CONSTRAINT_TYPE, - D6_SPRING_CONSTRAINT_TYPE, - GEAR_CONSTRAINT_TYPE, // added in Bullet 2.82 - FIXED_CONSTRAINT_TYPE, // added in Bullet 2.82 - MAX_CONSTRAINT_TYPE, // last type defined by Bullet + POINT2POINT_CONSTRAINT_TYPE = 3, + HINGE_CONSTRAINT_TYPE, + CONETWIST_CONSTRAINT_TYPE, + D6_CONSTRAINT_TYPE, + SLIDER_CONSTRAINT_TYPE, + CONTACT_CONSTRAINT_TYPE, + D6_SPRING_CONSTRAINT_TYPE, + GEAR_CONSTRAINT_TYPE, // added in Bullet 2.82 + FIXED_CONSTRAINT_TYPE, // added in Bullet 2.82 + MAX_CONSTRAINT_TYPE, // last type defined by Bullet // BS_FIXED_CONSTRAINT_TYPE = 1234 // BulletSim constraint that is fixed and unmoving } @@ -54,25 +54,25 @@ public enum ConstraintType : int [StructLayout(LayoutKind.Sequential)] public struct ConvexHull { - Vector3 Offset; - int VertexCount; - Vector3[] Vertices; + Vector3 Offset; + int VertexCount; + Vector3[] Vertices; } public enum BSPhysicsShapeType { - SHAPE_UNKNOWN = 0, - SHAPE_CAPSULE = 1, - SHAPE_BOX = 2, - SHAPE_CONE = 3, - SHAPE_CYLINDER = 4, - SHAPE_SPHERE = 5, - SHAPE_MESH = 6, - SHAPE_HULL = 7, + SHAPE_UNKNOWN = 0, + SHAPE_CAPSULE = 1, + SHAPE_BOX = 2, + SHAPE_CONE = 3, + SHAPE_CYLINDER = 4, + SHAPE_SPHERE = 5, + SHAPE_MESH = 6, + SHAPE_HULL = 7, // following defined by BulletSim - SHAPE_GROUNDPLANE = 20, - SHAPE_TERRAIN = 21, - SHAPE_COMPOUND = 22, - SHAPE_HEIGHTMAP = 23, + SHAPE_GROUNDPLANE = 20, + SHAPE_TERRAIN = 21, + SHAPE_COMPOUND = 22, + SHAPE_HEIGHTMAP = 23, SHAPE_AVATAR = 24, SHAPE_CONVEXHULL= 25, SHAPE_GIMPACT = 26, @@ -121,6 +121,14 @@ public struct SweepHit public float Fraction; public Vector3 Normal; public Vector3 Point; + + public bool hasHit() + { + float sum = Fraction + + Normal.X + Normal.Y + Normal.Z + + Point.X + Point.Y + Point.Z; + return (sum != 0) || (ID != 0); + } } [StructLayout(LayoutKind.Sequential)] public struct RaycastHit @@ -128,6 +136,13 @@ public struct RaycastHit public UInt32 ID; public float Fraction; public Vector3 Normal; + public Vector3 Point; + + public bool hasHit() + { + float sum = Normal.X + Normal.Y + Normal.Z + Point.X + Point.Y + Point.Z; + return (sum != 0); + } } [StructLayout(LayoutKind.Sequential)] public struct CollisionDesc @@ -180,16 +195,16 @@ public struct ConfigurationParameters public float collisionMargin; public float gravity; - public float maxPersistantManifoldPoolSize; - public float maxCollisionAlgorithmPoolSize; - public float shouldDisableContactPoolDynamicAllocation; - public float shouldForceUpdateAllAabbs; - public float shouldRandomizeSolverOrder; - public float shouldSplitSimulationIslands; - public float shouldEnableFrictionCaching; - public float numberOfSolverIterations; + public float maxPersistantManifoldPoolSize; + public float maxCollisionAlgorithmPoolSize; + public float shouldDisableContactPoolDynamicAllocation; + public float shouldForceUpdateAllAabbs; + public float shouldRandomizeSolverOrder; + public float shouldSplitSimulationIslands; + public float shouldEnableFrictionCaching; + public float numberOfSolverIterations; public float useSingleSidedMeshes; - public float globalContactBreakingThreshold; + public float globalContactBreakingThreshold; public float physicsLoggingFrames; @@ -202,30 +217,30 @@ public struct ConfigurationParameters public struct HACDParams { // usual default values - public float maxVerticesPerHull; // 100 - public float minClusters; // 2 - public float compacityWeight; // 0.1 - public float volumeWeight; // 0.0 - public float concavity; // 100 - public float addExtraDistPoints; // false - public float addNeighboursDistPoints; // false - public float addFacesPoints; // false - public float shouldAdjustCollisionMargin; // false + public float maxVerticesPerHull; // 100 + public float minClusters; // 2 + public float compacityWeight; // 0.1 + public float volumeWeight; // 0.0 + public float concavity; // 100 + public float addExtraDistPoints; // false + public float addNeighboursDistPoints; // false + public float addFacesPoints; // false + public float shouldAdjustCollisionMargin; // false // VHACD - public float whichHACD; // zero if Bullet HACD, non-zero says VHACD - // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html - public float vHACDresolution; // 100,000 max number of voxels generated during voxelization stage - public float vHACDdepth; // 20 max number of clipping stages - public float vHACDconcavity; // 0.0025 maximum concavity - public float vHACDplaneDownsampling; // 4 granularity of search for best clipping plane - public float vHACDconvexHullDownsampling; // 4 precision of hull gen process - public float vHACDalpha; // 0.05 bias toward clipping along symmetry planes - public float vHACDbeta; // 0.05 bias toward clipping along revolution axis - public float vHACDgamma; // 0.00125 max concavity when merging - public float vHACDpca; // 0 on/off normalizing mesh before decomp - public float vHACDmode; // 0 0:voxel based, 1: tetrahedron based - public float vHACDmaxNumVerticesPerCH; // 64 max triangles per convex hull - public float vHACDminVolumePerCH; // 0.0001 sampling of generated convex hulls + public float whichHACD; // zero if Bullet HACD, non-zero says VHACD + // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html + public float vHACDresolution; // 100,000 max number of voxels generated during voxelization stage + public float vHACDdepth; // 20 max number of clipping stages + public float vHACDconcavity; // 0.0025 maximum concavity + public float vHACDplaneDownsampling; // 4 granularity of search for best clipping plane + public float vHACDconvexHullDownsampling; // 4 precision of hull gen process + public float vHACDalpha; // 0.05 bias toward clipping along symmetry planes + public float vHACDbeta; // 0.05 bias toward clipping along revolution axis + public float vHACDgamma; // 0.00125 max concavity when merging + public float vHACDpca; // 0 on/off normalizing mesh before decomp + public float vHACDmode; // 0 0:voxel based, 1: tetrahedron based + public float vHACDmaxNumVerticesPerCH; // 64 max triangles per convex hull + public float vHACDminVolumePerCH; // 0.0001 sampling of generated convex hulls } // The states a bullet collision object can have @@ -322,8 +337,8 @@ public abstract string BulletEngineVersion { get; protected set;} // Initialization and simulation public abstract BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms, - int maxCollisions, ref CollisionDesc[] collisionArray, - int maxUpdates, ref EntityProperties[] updateArray + int maxCollisions, ref CollisionDesc[] collisionArray, + int maxUpdates, ref EntityProperties[] updateArray ); public abstract int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep, @@ -398,7 +413,7 @@ public abstract void DestroyObject(BulletWorld sim, BulletBody obj); public abstract BulletShape CreateGroundPlaneShape(UInt32 id, float height, float collisionMargin); public abstract BulletShape CreateTerrainShape(UInt32 id, Vector3 size, float minHeight, float maxHeight, float[] heightMap, - float scaleFactor, float collisionMargin); + float scaleFactor, float collisionMargin); // ===================================================================================== // Constraint creation and helper routines @@ -742,6 +757,12 @@ public abstract void SetMargin(BulletShape shape, float val); public abstract float GetMargin(BulletShape shape); // ===================================================================================== +// Raycast +public abstract SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin); + +public abstract RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask); + +// ===================================================================================== // Debugging public virtual void DumpRigidBody(BulletWorld sim, BulletBody collisionObject) { } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs index 83fc3a6..d182c34 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs @@ -40,7 +40,6 @@ public sealed class BSCharacter : BSPhysObject private static readonly string LogHeader = "[BULLETS CHAR]"; // private bool _stopped; - private OMV.Vector3 _size; private bool _grabbed; private bool _selected; private float _mass; @@ -53,19 +52,21 @@ public sealed class BSCharacter : BSPhysObject private bool _setAlwaysRun; private bool _throttleUpdates; private bool _floatOnWater; - private OMV.Vector3 _rotationalVelocity; private bool _kinematic; private float _buoyancy; + private OMV.Vector3 _size; + private float _footOffset; + private BSActorAvatarMove m_moveActor; private const string AvatarMoveActorName = "BSCharacter.AvatarMove"; private OMV.Vector3 _PIDTarget; private float _PIDTau; -// public override OMV.Vector3 RawVelocity -// { get { return base.RawVelocity; } -// set { +// public override OMV.Vector3 RawVelocity +// { get { return base.RawVelocity; } +// set { // if (value != base.RawVelocity) // Util.PrintCallStack(); // Console.WriteLine("Set rawvel to {0}", value); @@ -76,12 +77,12 @@ public sealed class BSCharacter : BSPhysObject public override bool IsIncomplete { get { return false; } } public BSCharacter( - uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 vel, OMV.Vector3 size, bool isFlying) + uint localID, String avName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 vel, OMV.Vector3 size, float footOffset, bool isFlying) : base(parent_scene, localID, avName, "BSCharacter") { _physicsActorType = (int)ActorTypes.Agent; - RawPosition = pos; + RawPosition = pos; _flying = isFlying; RawOrientation = OMV.Quaternion.Identity; @@ -89,22 +90,15 @@ public sealed class BSCharacter : BSPhysObject _buoyancy = ComputeBuoyancyFromFlying(isFlying); Friction = BSParam.AvatarStandingFriction; Density = BSParam.AvatarDensity; + _isPhysical = true; - // Old versions of ScenePresence passed only the height. If width and/or depth are zero, - // replace with the default values. - _size = size; - if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth; - if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth; - - // The dimensions of the physical capsule are kept in the scale. - // Physics creates a unit capsule which is scaled by the physics engine. - Scale = ComputeAvatarScale(_size); - // set _avatarVolume and _mass based on capsule size, _density and Scale - ComputeAvatarVolumeAndMass(); + // Adjustments for zero X and Y made in Size() + // This also computes avatar scale, volume, and mass + SetAvatarSize(size, footOffset, true /* initializing */); DetailLog( "{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5},pos={6},vel={7}", - LocalID, _size, Scale, Density, _avatarVolume, RawMass, pos, vel); + LocalID, Size, Scale, Density, _avatarVolume, RawMass, pos, vel); // do actual creation in taint time PhysScene.TaintedObject(LocalID, "BSCharacter.create", delegate() @@ -208,45 +202,68 @@ public sealed class BSCharacter : BSPhysObject public override OMV.Vector3 Size { get { - // Avatar capsule size is kept in the scale parameter. return _size; } set { - // This is how much the avatar size is changing. Positive means getting bigger. - // The avatar altitude must be adjusted for this change. - float heightChange = value.Z - _size.Z; + setAvatarSize(value, _footOffset); + } + } + + // OpenSim 0.9 introduces a common avatar size computation + public override void setAvatarSize(OMV.Vector3 size, float feetOffset) + { + SetAvatarSize(size, feetOffset, false /* initializing */); + } - _size = value; + // Internal version that, if initializing, doesn't do all the updating of the physics engine + public void SetAvatarSize(OMV.Vector3 size, float feetOffset, bool initializing) + { + OMV.Vector3 newSize = size; + if (newSize.IsFinite()) + { // Old versions of ScenePresence passed only the height. If width and/or depth are zero, // replace with the default values. - if (_size.X == 0f) _size.X = BSParam.AvatarCapsuleDepth; - if (_size.Y == 0f) _size.Y = BSParam.AvatarCapsuleWidth; + if (newSize.X == 0f) newSize.X = BSParam.AvatarCapsuleDepth; + if (newSize.Y == 0f) newSize.Y = BSParam.AvatarCapsuleWidth; - Scale = ComputeAvatarScale(_size); - ComputeAvatarVolumeAndMass(); - DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}", - LocalID, _size, Scale, Density, _avatarVolume, RawMass); + if (newSize.X < 0.01f) newSize.X = 0.01f; + if (newSize.Y < 0.01f) newSize.Y = 0.01f; + if (newSize.Z < 0.01f) newSize.Z = BSParam.AvatarCapsuleHeight; + } + else + { + newSize = new OMV.Vector3(BSParam.AvatarCapsuleDepth, BSParam.AvatarCapsuleWidth, BSParam.AvatarCapsuleHeight); + } - PhysScene.TaintedObject(LocalID, "BSCharacter.setSize", delegate() - { - if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape) - { - PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale); - UpdatePhysicalMassProperties(RawMass, true); + // This is how much the avatar size is changing. Positive means getting bigger. + // The avatar altitude must be adjusted for this change. + float heightChange = newSize.Z - Size.Z; - // Adjust the avatar's position to account for the increase/decrease in size - ForcePosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, RawPosition.Z + heightChange / 2f); + _size = newSize; - // Make sure this change appears as a property update event - PhysScene.PE.PushUpdate(PhysBody); - } - }); + Scale = ComputeAvatarScale(Size); + ComputeAvatarVolumeAndMass(); + DetailLog("{0},BSCharacter.setSize,call,size={1},scale={2},density={3},volume={4},mass={5}", + LocalID, _size, Scale, Density, _avatarVolume, RawMass); - } + PhysScene.TaintedObject(LocalID, "BSCharacter.setSize", delegate() + { + if (PhysBody.HasPhysicalBody && PhysShape.physShapeInfo.HasPhysicalShape) + { + PhysScene.PE.SetLocalScaling(PhysShape.physShapeInfo, Scale); + UpdatePhysicalMassProperties(RawMass, true); + + // Adjust the avatar's position to account for the increase/decrease in size + ForcePosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, RawPosition.Z + heightChange / 2f); + + // Make sure this change appears as a property update event + PhysScene.PE.PushUpdate(PhysBody); + } + }); } - public override PrimitiveBaseShape Shape + public override PrimitiveBaseShape Shape { set { BaseShape = value; } } @@ -273,7 +290,7 @@ public sealed class BSCharacter : BSPhysObject { RawVelocity = OMV.Vector3.Zero; _acceleration = OMV.Vector3.Zero; - _rotationalVelocity = OMV.Vector3.Zero; + RawRotationalVelocity = OMV.Vector3.Zero; // Zero some other properties directly into the physics engine PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.ZeroMotion", delegate() @@ -285,7 +302,7 @@ public sealed class BSCharacter : BSPhysObject public override void ZeroAngularMotion(bool inTaintTime) { - _rotationalVelocity = OMV.Vector3.Zero; + RawRotationalVelocity = OMV.Vector3.Zero; PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.ZeroMotion", delegate() { @@ -300,7 +317,7 @@ public sealed class BSCharacter : BSPhysObject } - public override void LockAngularMotion(OMV.Vector3 axis) { return; } + public override void LockAngularMotion(byte axislocks) { return; } public override OMV.Vector3 Position { get { @@ -333,7 +350,6 @@ public sealed class BSCharacter : BSPhysObject } } - // Check that the current position is sane and, if not, modify the position to make it so. // Check for being below terrain or on water. // Returns 'true' of the position was made sane by some action. @@ -433,6 +449,7 @@ public sealed class BSCharacter : BSPhysObject public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } } public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } } + // PhysicsActor.TargetVelocity // Sets the target in the motor. This starts the changing of the avatar's velocity. public override OMV.Vector3 TargetVelocity { @@ -443,43 +460,51 @@ public sealed class BSCharacter : BSPhysObject set { DetailLog("{0},BSCharacter.setTargetVelocity,call,vel={1}", LocalID, value); - m_targetVelocity = value; OMV.Vector3 targetVel = value; - if (_setAlwaysRun && !_flying) - targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 1f); + if (!_flying) + { + if (_setAlwaysRun) + targetVel *= new OMV.Vector3(BSParam.AvatarAlwaysRunFactor, BSParam.AvatarAlwaysRunFactor, 1f); + else + if (BSParam.AvatarWalkVelocityFactor != 1f) + targetVel *= new OMV.Vector3(BSParam.AvatarWalkVelocityFactor, BSParam.AvatarWalkVelocityFactor, 1f); + } + base.m_targetVelocity = targetVel; if (m_moveActor != null) - m_moveActor.SetVelocityAndTarget(RawVelocity, targetVel, false /* inTaintTime */); + m_moveActor.SetVelocityAndTarget(RawVelocity, base.m_targetVelocity, false /* inTaintTime */); } } // Directly setting velocity means this is what the user really wants now. public override OMV.Vector3 Velocity { get { return RawVelocity; } set { - RawVelocity = value; - OMV.Vector3 vel = RawVelocity; - - DetailLog("{0}: set Velocity = {1}", LocalID, value); - - PhysScene.TaintedObject(LocalID, "BSCharacter.setVelocity", delegate() + if (m_moveActor != null) { - if (m_moveActor != null) - m_moveActor.SetVelocityAndTarget(vel, vel, true /* inTaintTime */); + // m_moveActor.SetVelocityAndTarget(OMV.Vector3.Zero, OMV.Vector3.Zero, false /* inTaintTime */); + m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false /* inTaintTime */); + } + base.Velocity = value; + } + } - DetailLog("{0},BSCharacter.setVelocity,taint,vel={1}", LocalID, vel); - ForceVelocity = vel; - }); + // SetMomentum just sets the velocity without a target. We need to stop the movement actor if a character. + public override void SetMomentum(OMV.Vector3 momentum) + { + if (m_moveActor != null) + { + // m_moveActor.SetVelocityAndTarget(OMV.Vector3.Zero, OMV.Vector3.Zero, false /* inTaintTime */); + m_moveActor.SetVelocityAndTarget(RawVelocity, RawVelocity, false /* inTaintTime */); } + base.SetMomentum(momentum); } public override OMV.Vector3 ForceVelocity { get { return RawVelocity; } set { - PhysScene.AssertInTaintTime("BSCharacter.ForceVelocity"); -// Util.PrintCallStack(); - DetailLog("{0}: set ForceVelocity = {1}", LocalID, value); + DetailLog("{0},BSCharacter.ForceVelocity.set={1}", LocalID, value); - RawVelocity = value; + RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity); PhysScene.PE.SetLinearVelocity(PhysBody, RawVelocity); PhysScene.PE.Activate(PhysBody, true); } @@ -600,14 +625,6 @@ public sealed class BSCharacter : BSPhysObject }); } } - public override OMV.Vector3 RotationalVelocity { - get { return _rotationalVelocity; } - set { _rotationalVelocity = value; } - } - public override OMV.Vector3 ForceRotationalVelocity { - get { return _rotationalVelocity; } - set { _rotationalVelocity = value; } - } public override bool Kinematic { get { return _kinematic; } set { _kinematic = value; } @@ -626,8 +643,6 @@ public sealed class BSCharacter : BSPhysObject public override float ForceBuoyancy { get { return _buoyancy; } set { - PhysScene.AssertInTaintTime("BSCharacter.ForceBuoyancy"); - _buoyancy = value; DetailLog("{0},BSCharacter.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy); // Buoyancy is faked by changing the gravity applied to the object @@ -652,14 +667,24 @@ public sealed class BSCharacter : BSPhysObject public override void AddForce(OMV.Vector3 force, bool pushforce) { // Since this force is being applied in only one step, make this a force per second. - OMV.Vector3 addForce = force / PhysScene.LastTimeStep; - AddForce(addForce, pushforce, false); + OMV.Vector3 addForce = force; + + // The interaction of this force with the simulator rate and collision occurance is tricky. + // ODE multiplies the force by 100 + // ubODE multiplies the force by 5.3 + // BulletSim, after much in-world testing, thinks it gets a similar effect by multiplying mass*0.315f + // This number could be a feature of friction or timing, but it seems to move avatars the same as ubODE + addForce *= Mass * BSParam.AvatarAddForcePushFactor; + + DetailLog("{0},BSCharacter.addForce,call,force={1},addForce={2},push={3},mass={4}", LocalID, force, addForce, pushforce, Mass); + AddForce(false, addForce); } - public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { + + public override void AddForce(bool inTaintTime, OMV.Vector3 force) { if (force.IsFinite()) { OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude); - // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce); + // DetailLog("{0},BSCharacter.addForce,call,force={1},push={2},inTaint={3}", LocalID, addForce, pushforce, inTaintTime); PhysScene.TaintedObject(inTaintTime, LocalID, "BSCharacter.AddForce", delegate() { @@ -667,7 +692,15 @@ public sealed class BSCharacter : BSPhysObject // DetailLog("{0},BSCharacter.addForce,taint,force={1}", LocalID, addForce); if (PhysBody.HasPhysicalBody) { + // Bullet adds this central force to the total force for this tick. + // Deep down in Bullet: + // linearVelocity += totalForce / mass * timeStep; PhysScene.PE.ApplyCentralForce(PhysBody, addForce); + PhysScene.PE.Activate(PhysBody, true); + } + if (m_moveActor != null) + { + m_moveActor.SuppressStationayCheckUntilLowVelocity(); } }); } @@ -678,65 +711,74 @@ public sealed class BSCharacter : BSPhysObject } } - public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { - } - public override void SetMomentum(OMV.Vector3 momentum) { + public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force) { } + // The avatar's physical shape (whether capsule or cube) is unit sized. BulletSim sets + // the scale of that unit shape to create the avatars full size. private OMV.Vector3 ComputeAvatarScale(OMV.Vector3 size) { OMV.Vector3 newScale = size; - // Bullet's capsule total height is the "passed height + radius * 2"; - // The base capsule is 1 unit in diameter and 2 units in height (passed radius=0.5, passed height = 1) - // The number we pass in for 'scaling' is the multiplier to get that base - // shape to be the size desired. - // So, when creating the scale for the avatar height, we take the passed height - // (size.Z) and remove the caps. - // An oddity of the Bullet capsule implementation is that it presumes the Y - // dimension is the radius of the capsule. Even though some of the code allows - // for a asymmetrical capsule, other parts of the code presume it is cylindrical. + if (BSParam.AvatarUseBefore09SizeComputation) + { - // Scale is multiplier of radius with one of "0.5" + // Bullet's capsule total height is the "passed height + radius * 2"; + // The base capsule is 1 unit in diameter and 2 units in height (passed radius=0.5, passed height = 1) + // The number we pass in for 'scaling' is the multiplier to get that base + // shape to be the size desired. + // So, when creating the scale for the avatar height, we take the passed height + // (size.Z) and remove the caps. + // An oddity of the Bullet capsule implementation is that it presumes the Y + // dimension is the radius of the capsule. Even though some of the code allows + // for a asymmetrical capsule, other parts of the code presume it is cylindrical. - float heightAdjust = BSParam.AvatarHeightMidFudge; - if (BSParam.AvatarHeightLowFudge != 0f || BSParam.AvatarHeightHighFudge != 0f) - { - const float AVATAR_LOW = 1.1f; - const float AVATAR_MID = 1.775f; // 1.87f - const float AVATAR_HI = 2.45f; - // An avatar is between 1.1 and 2.45 meters. Midpoint is 1.775m. - float midHeightOffset = size.Z - AVATAR_MID; - if (midHeightOffset < 0f) + // Scale is multiplier of radius with one of "0.5" + + float heightAdjust = BSParam.AvatarHeightMidFudge; + if (BSParam.AvatarHeightLowFudge != 0f || BSParam.AvatarHeightHighFudge != 0f) { - // Small avatar. Add the adjustment based on the distance from midheight - heightAdjust += ((-1f * midHeightOffset) / (AVATAR_MID - AVATAR_LOW)) * BSParam.AvatarHeightLowFudge; + const float AVATAR_LOW = 1.1f; + const float AVATAR_MID = 1.775f; // 1.87f + const float AVATAR_HI = 2.45f; + // An avatar is between 1.1 and 2.45 meters. Midpoint is 1.775m. + float midHeightOffset = size.Z - AVATAR_MID; + if (midHeightOffset < 0f) + { + // Small avatar. Add the adjustment based on the distance from midheight + heightAdjust += ((-1f * midHeightOffset) / (AVATAR_MID - AVATAR_LOW)) * BSParam.AvatarHeightLowFudge; + } + else + { + // Large avatar. Add the adjustment based on the distance from midheight + heightAdjust += ((midHeightOffset) / (AVATAR_HI - AVATAR_MID)) * BSParam.AvatarHeightHighFudge; + } + } + if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule) + { + newScale.X = size.X / 2f; + newScale.Y = size.Y / 2f; + // The total scale height is the central cylindar plus the caps on the two ends. + newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2) + heightAdjust) / 2f; } else { - // Large avatar. Add the adjustment based on the distance from midheight - heightAdjust += ((midHeightOffset) / (AVATAR_HI - AVATAR_MID)) * BSParam.AvatarHeightHighFudge; + newScale.Z = size.Z + heightAdjust; } - } - if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule) - { - newScale.X = size.X / 2f; - newScale.Y = size.Y / 2f; - // The total scale height is the central cylindar plus the caps on the two ends. - newScale.Z = (size.Z + (Math.Min(size.X, size.Y) * 2) + heightAdjust) / 2f; + // m_log.DebugFormat("{0} ComputeAvatarScale: size={1},adj={2},scale={3}", LogHeader, size, heightAdjust, newScale); + + // If smaller than the endcaps, just fake like we're almost that small + if (newScale.Z < 0) + newScale.Z = 0.1f; + + DetailLog("{0},BSCharacter.ComputeAvatarScale,size={1},lowF={2},midF={3},hiF={4},adj={5},newScale={6}", + LocalID, size, BSParam.AvatarHeightLowFudge, BSParam.AvatarHeightMidFudge, BSParam.AvatarHeightHighFudge, heightAdjust, newScale); } else { - newScale.Z = size.Z + heightAdjust; + newScale.Z = size.Z + _footOffset; + DetailLog("{0},BSCharacter.ComputeAvatarScale,using newScale={1}, footOffset={2}", LocalID, newScale, _footOffset); } - // m_log.DebugFormat("{0} ComputeAvatarScale: size={1},adj={2},scale={3}", LogHeader, size, heightAdjust, newScale); - - // If smaller than the endcaps, just fake like we're almost that small - if (newScale.Z < 0) - newScale.Z = 0.1f; - - DetailLog("{0},BSCharacter.ComputerAvatarScale,size={1},lowF={2},midF={3},hiF={4},adj={5},newScale={6}", - LocalID, size, BSParam.AvatarHeightLowFudge, BSParam.AvatarHeightMidFudge, BSParam.AvatarHeightHighFudge, heightAdjust, newScale); return newScale; } @@ -744,17 +786,24 @@ public sealed class BSCharacter : BSPhysObject // set _avatarVolume and _mass based on capsule size, _density and Scale private void ComputeAvatarVolumeAndMass() { - _avatarVolume = (float)( - Math.PI - * Size.X / 2f - * Size.Y / 2f // the area of capsule cylinder - * Size.Z // times height of capsule cylinder - + 1.33333333f - * Math.PI - * Size.X / 2f - * Math.Min(Size.X, Size.Y) / 2 - * Size.Y / 2f // plus the volume of the capsule end caps - ); + if (BSParam.AvatarShape == BSShapeCollection.AvatarShapeCapsule) + { + _avatarVolume = (float)( + Math.PI + * Size.X / 2f + * Size.Y / 2f // the area of capsule cylinder + * Size.Z // times height of capsule cylinder + + 1.33333333f + * Math.PI + * Size.X / 2f + * Math.Min(Size.X, Size.Y) / 2 + * Size.Y / 2f // plus the volume of the capsule end caps + ); + } + else + { + _avatarVolume = Size.X * Size.Y * Size.Z; + } _mass = Density * BSParam.DensityScaleFactor * _avatarVolume; } @@ -773,7 +822,7 @@ public sealed class BSCharacter : BSPhysObject // 0.001m/s. Bullet introduces a lot of jitter in the velocity which causes many // extra updates. // - // XXX: Contrary to the above comment, setting an update threshold here above 0.4 actually introduces jitter to + // XXX: Contrary to the above comment, setting an update threshold here above 0.4 actually introduces jitter to // avatar movement rather than removes it. The larger the threshold, the bigger the jitter. // This is most noticeable in level flight and can be seen with // the "show updates" option in a viewer. With an update threshold, the RawVelocity cycles between a lower @@ -787,7 +836,7 @@ public sealed class BSCharacter : BSPhysObject RawVelocity = entprop.Velocity; _acceleration = entprop.Acceleration; - _rotationalVelocity = entprop.RotationalVelocity; + RawRotationalVelocity = entprop.RotationalVelocity; // Do some sanity checking for the avatar. Make sure it's above ground and inbounds. if (PositionSanityCheck(true)) @@ -807,7 +856,7 @@ public sealed class BSCharacter : BSPhysObject // PhysScene.PostUpdate(this); DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}", - LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, _rotationalVelocity); + LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, RawRotationalVelocity); } } } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraint6Dof.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintCollection.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintConeTwist.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintHinge.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSlider.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs b/OpenSim/Region/PhysicsModules/BulletS/BSConstraintSpring.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs index 0fc5577..c4ccdbe 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs @@ -125,8 +125,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS // Just some recomputed constants: #pragma warning disable 414 - static readonly float TwoPI = ((float)Math.PI) * 2f; - static readonly float FourPI = ((float)Math.PI) * 4f; + static readonly float TwoPI = ((float)Math.PI) * 2f; + static readonly float FourPI = ((float)Math.PI) * 4f; static readonly float PIOverFour = ((float)Math.PI) / 4f; static readonly float PIOverTwo = ((float)Math.PI) / 2f; #pragma warning restore 414 @@ -768,7 +768,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS } if ((m_knownChanged & m_knownChangedForce) != 0) - ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/); + ControllingPrim.AddForce(false /* inTaintTime */, (Vector3)m_knownForce); if ((m_knownChanged & m_knownChangedForceImpulse) != 0) ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/); @@ -784,7 +784,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS if ((m_knownChanged & m_knownChangedRotationalForce) != 0) { - ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/); + ControllingPrim.AddAngularForce(true /* inTaintTime */, (Vector3)m_knownRotationalForce); } // If we set one of the values (ie, the physics engine didn't do it) we must force @@ -1595,7 +1595,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS // in that direction. // TODO: implement reference frame. public void ComputeAngularDeflection() - { + { if (BSParam.VehicleEnableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2) { diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs old mode 100644 new mode 100755 index 8312239..13c1361 --- a/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinkset.cs @@ -257,7 +257,7 @@ public abstract class BSLinkset // Return 'true' if linkset processed the collision. 'false' says the linkset didn't have // anything to add for the collision and it should be passed through normal processing. // Default processing for a linkset. - public virtual bool HandleCollide(uint collidingWith, BSPhysObject collidee, + public virtual bool HandleCollide(BSPhysObject collider, BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) { bool ret = false; @@ -319,7 +319,7 @@ public abstract class BSLinkset public virtual bool AllPartsComplete { get { - bool ret = true; + bool ret = true; this.ForEachMember((member) => { if ((!member.IsInitialized) || member.IsIncomplete || member.PrimAssetState == BSPhysObject.PrimAssetCondition.Waiting) diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs old mode 100644 new mode 100755 index 953ddee..dc390b2 --- a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetCompound.cs @@ -450,6 +450,7 @@ public sealed class BSLinksetCompound : BSLinkset m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody); DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}", LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape); + m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World); // DEBUG DEBUG // With all of the linkset packed into the root prim, it has the mass of everyone. LinksetMass = ComputeLinksetMass(); diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs b/OpenSim/Region/PhysicsModules/BulletS/BSLinksetConstraints.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs b/OpenSim/Region/PhysicsModules/BulletS/BSMaterials.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs b/OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs old mode 100644 new mode 100755 index c296008..495f752 --- a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs @@ -90,8 +90,8 @@ public static class BSParam public static float DeactivationTime { get; private set; } public static float LinearSleepingThreshold { get; private set; } public static float AngularSleepingThreshold { get; private set; } - public static float CcdMotionThreshold { get; private set; } - public static float CcdSweptSphereRadius { get; private set; } + public static float CcdMotionThreshold { get; private set; } + public static float CcdSweptSphereRadius { get; private set; } public static float ContactProcessingThreshold { get; private set; } public static bool ShouldMeshSculptedPrim { get; private set; } // cause scuplted prims to get meshed @@ -119,14 +119,14 @@ public static class BSParam public static float Gravity { get; private set; } // Physics Engine operation - public static float MaxPersistantManifoldPoolSize { get; private set; } - public static float MaxCollisionAlgorithmPoolSize { get; private set; } - public static bool ShouldDisableContactPoolDynamicAllocation { get; private set; } - public static bool ShouldForceUpdateAllAabbs { get; private set; } - public static bool ShouldRandomizeSolverOrder { get; private set; } - public static bool ShouldSplitSimulationIslands { get; private set; } - public static bool ShouldEnableFrictionCaching { get; private set; } - public static float NumberOfSolverIterations { get; private set; } + public static float MaxPersistantManifoldPoolSize { get; private set; } + public static float MaxCollisionAlgorithmPoolSize { get; private set; } + public static bool ShouldDisableContactPoolDynamicAllocation { get; private set; } + public static bool ShouldForceUpdateAllAabbs { get; private set; } + public static bool ShouldRandomizeSolverOrder { get; private set; } + public static bool ShouldSplitSimulationIslands { get; private set; } + public static bool ShouldEnableFrictionCaching { get; private set; } + public static float NumberOfSolverIterations { get; private set; } public static bool UseSingleSidedMeshes { get; private set; } public static float GlobalContactBreakingThreshold { get; private set; } public static float PhysicsUnmanLoggingFrames { get; private set; } @@ -135,6 +135,7 @@ public static class BSParam public static bool AvatarToAvatarCollisionsByDefault { get; private set; } public static float AvatarFriction { get; private set; } public static float AvatarStandingFriction { get; private set; } + public static float AvatarWalkVelocityFactor { get; private set; } public static float AvatarAlwaysRunFactor { get; private set; } public static float AvatarDensity { get; private set; } public static float AvatarRestitution { get; private set; } @@ -142,23 +143,26 @@ public static class BSParam public static float AvatarCapsuleWidth { get; private set; } public static float AvatarCapsuleDepth { get; private set; } public static float AvatarCapsuleHeight { get; private set; } + public static bool AvatarUseBefore09SizeComputation { get; private set; } public static float AvatarHeightLowFudge { get; private set; } public static float AvatarHeightMidFudge { get; private set; } public static float AvatarHeightHighFudge { get; private set; } public static float AvatarFlyingGroundMargin { get; private set; } public static float AvatarFlyingGroundUpForce { get; private set; } public static float AvatarTerminalVelocity { get; private set; } - public static float AvatarContactProcessingThreshold { get; private set; } + public static float AvatarContactProcessingThreshold { get; private set; } + public static float AvatarAddForcePushFactor { get; private set; } public static float AvatarStopZeroThreshold { get; private set; } - public static int AvatarJumpFrames { get; private set; } - public static float AvatarBelowGroundUpCorrectionMeters { get; private set; } - public static float AvatarStepHeight { get; private set; } - public static float AvatarStepAngle { get; private set; } - public static float AvatarStepGroundFudge { get; private set; } - public static float AvatarStepApproachFactor { get; private set; } - public static float AvatarStepForceFactor { get; private set; } - public static float AvatarStepUpCorrectionFactor { get; private set; } - public static int AvatarStepSmoothingSteps { get; private set; } + public static float AvatarStopZeroThresholdSquared { get; private set; } + public static int AvatarJumpFrames { get; private set; } + public static float AvatarBelowGroundUpCorrectionMeters { get; private set; } + public static float AvatarStepHeight { get; private set; } + public static float AvatarStepAngle { get; private set; } + public static float AvatarStepGroundFudge { get; private set; } + public static float AvatarStepApproachFactor { get; private set; } + public static float AvatarStepForceFactor { get; private set; } + public static float AvatarStepUpCorrectionFactor { get; private set; } + public static int AvatarStepSmoothingSteps { get; private set; } // Vehicle parameters public static float VehicleMaxLinearVelocity { get; private set; } @@ -190,31 +194,31 @@ public static class BSParam public static float CSHullVolumeConservationThresholdPercent { get; private set; } public static int CSHullMaxVertices { get; private set; } public static float CSHullMaxSkinWidth { get; private set; } - public static float BHullMaxVerticesPerHull { get; private set; } // 100 - public static float BHullMinClusters { get; private set; } // 2 - public static float BHullCompacityWeight { get; private set; } // 0.1 - public static float BHullVolumeWeight { get; private set; } // 0.0 - public static float BHullConcavity { get; private set; } // 100 - public static bool BHullAddExtraDistPoints { get; private set; } // false - public static bool BHullAddNeighboursDistPoints { get; private set; } // false - public static bool BHullAddFacesPoints { get; private set; } // false - public static bool BHullShouldAdjustCollisionMargin { get; private set; } // false - public static float WhichHACD { get; private set; } // zero if Bullet HACD, non-zero says VHACD + public static float BHullMaxVerticesPerHull { get; private set; } // 100 + public static float BHullMinClusters { get; private set; } // 2 + public static float BHullCompacityWeight { get; private set; } // 0.1 + public static float BHullVolumeWeight { get; private set; } // 0.0 + public static float BHullConcavity { get; private set; } // 100 + public static bool BHullAddExtraDistPoints { get; private set; } // false + public static bool BHullAddNeighboursDistPoints { get; private set; } // false + public static bool BHullAddFacesPoints { get; private set; } // false + public static bool BHullShouldAdjustCollisionMargin { get; private set; } // false + public static float WhichHACD { get; private set; } // zero if Bullet HACD, non-zero says VHACD // Parameters for VHACD 2.0: http://code.google.com/p/v-hacd // To enable, set both ShouldUseBulletHACD=true and WhichHACD=1 - // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html - public static float VHACDresolution { get; private set; } // 100,000 max number of voxels generated during voxelization stage - public static float VHACDdepth { get; private set; } // 20 max number of clipping stages - public static float VHACDconcavity { get; private set; } // 0.0025 maximum concavity - public static float VHACDplaneDownsampling { get; private set; } // 4 granularity of search for best clipping plane - public static float VHACDconvexHullDownsampling { get; private set; } // 4 precision of hull gen process - public static float VHACDalpha { get; private set; } // 0.05 bias toward clipping along symmetry planes - public static float VHACDbeta { get; private set; } // 0.05 bias toward clipping along revolution axis - public static float VHACDgamma { get; private set; } // 0.00125 max concavity when merging - public static float VHACDpca { get; private set; } // 0 on/off normalizing mesh before decomp - public static float VHACDmode { get; private set; } // 0 0:voxel based, 1: tetrahedron based - public static float VHACDmaxNumVerticesPerCH { get; private set; } // 64 max triangles per convex hull - public static float VHACDminVolumePerCH { get; private set; } // 0.0001 sampling of generated convex hulls + // http://kmamou.blogspot.ca/2014/12/v-hacd-20-parameters-description.html + public static float VHACDresolution { get; private set; } // 100,000 max number of voxels generated during voxelization stage + public static float VHACDdepth { get; private set; } // 20 max number of clipping stages + public static float VHACDconcavity { get; private set; } // 0.0025 maximum concavity + public static float VHACDplaneDownsampling { get; private set; } // 4 granularity of search for best clipping plane + public static float VHACDconvexHullDownsampling { get; private set; } // 4 precision of hull gen process + public static float VHACDalpha { get; private set; } // 0.05 bias toward clipping along symmetry planes + public static float VHACDbeta { get; private set; } // 0.05 bias toward clipping along revolution axis + public static float VHACDgamma { get; private set; } // 0.00125 max concavity when merging + public static float VHACDpca { get; private set; } // 0 on/off normalizing mesh before decomp + public static float VHACDmode { get; private set; } // 0 0:voxel based, 1: tetrahedron based + public static float VHACDmaxNumVerticesPerCH { get; private set; } // 64 max triangles per convex hull + public static float VHACDminVolumePerCH { get; private set; } // 0.0001 sampling of generated convex hulls // Linkset implementation parameters public static float LinksetImplementation { get; private set; } @@ -227,9 +231,13 @@ public static class BSParam public static float LinkConstraintCFM { get; private set; } public static float LinkConstraintSolverIterations { get; private set; } + public static bool UseBulletRaycast { get; private set; } + public static float PID_D { get; private set; } // derivative public static float PID_P { get; private set; } // proportional + public static float DebugNumber { get; private set; } // A console setable number used for debugging + // Various constants that come from that other virtual world that shall not be named. public const float MinGravityZ = -1f; public const float MaxGravityZ = 28f; @@ -574,7 +582,7 @@ public static class BSParam (s,v) => { ContactProcessingThreshold = v;}, (s,o) => { s.PE.SetContactProcessingThreshold(o.PhysBody, ContactProcessingThreshold); } ), - new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)", + new ParameterDefn("TerrainImplementation", "Type of shape to use for terrain (0=heightmap, 1=mesh)", (float)BSTerrainPhys.TerrainImplementation.Heightmap ), new ParameterDefn("TerrainMeshMagnification", "Number of times the 256x256 heightmap is multiplied to create the terrain mesh" , 2 ), @@ -597,11 +605,13 @@ public static class BSParam 0.2f ), new ParameterDefn("AvatarStandingFriction", "Avatar friction when standing. Changed on avatar recreation.", 0.95f ), + new ParameterDefn("AvatarWalkVelocityFactor", "Speed multiplier if avatar is walking", + 1.0f ), new ParameterDefn("AvatarAlwaysRunFactor", "Speed multiplier if avatar is set to always run", 1.3f ), // For historical reasons, density is reported * 100 new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation. Scaled times 100.", - 3500f) , // 3.5 * 100 + 350f) , // 3.5 * 100 new ParameterDefn("AvatarRestitution", "Bouncyness. Changed on avatar recreation.", 0f ), new ParameterDefn("AvatarShape", "Code for avatar physical shape: 0:capsule, 1:cube, 2:ovoid, 2:mesh", @@ -612,6 +622,8 @@ public static class BSParam 0.45f ), new ParameterDefn("AvatarCapsuleHeight", "Default height of space around avatar", 1.5f ), + new ParameterDefn("AvatarUseBefore09SizeComputation", "Use the old fudge method of computing avatar capsule size", + true ), new ParameterDefn("AvatarHeightLowFudge", "A fudge factor to make small avatars stand on the ground", 0f ), new ParameterDefn("AvatarHeightMidFudge", "A fudge distance to adjust average sized avatars to be standing on ground", @@ -624,27 +636,31 @@ public static class BSParam 2.0f ), new ParameterDefn("AvatarTerminalVelocity", "Terminal Velocity of falling avatar", -54.0f ), - new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions", - 0.1f ), - new ParameterDefn("AvatarStopZeroThreshold", "Movement velocity below which avatar is assumed to be stopped", + new ParameterDefn("AvatarContactProcessingThreshold", "Distance from capsule to check for collisions", 0.1f ), - new ParameterDefn("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground", + new ParameterDefn("AvatarAddForcePushFactor", "BSCharacter.AddForce is multiplied by this and mass to be like other physics engines", + 0.315f ), + new ParameterDefn("AvatarStopZeroThreshold", "Movement velocity below which avatar is assumed to be stopped", + 0.45f, + (s) => { return (float)AvatarStopZeroThreshold; }, + (s,v) => { AvatarStopZeroThreshold = v; AvatarStopZeroThresholdSquared = v * v; } ), + new ParameterDefn("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground", 1.0f ), - new ParameterDefn("AvatarJumpFrames", "Number of frames to allow jump forces. Changes jump height.", + new ParameterDefn("AvatarJumpFrames", "Number of frames to allow jump forces. Changes jump height.", 4 ), - new ParameterDefn("AvatarStepHeight", "Height of a step obstacle to consider step correction", + new ParameterDefn("AvatarStepHeight", "Height of a step obstacle to consider step correction", 0.999f ) , - new ParameterDefn("AvatarStepAngle", "The angle (in radians) for a vertical surface to be considered a step", + new ParameterDefn("AvatarStepAngle", "The angle (in radians) for a vertical surface to be considered a step", 0.3f ) , - new ParameterDefn("AvatarStepGroundFudge", "Fudge factor subtracted from avatar base when comparing collision height", + new ParameterDefn("AvatarStepGroundFudge", "Fudge factor subtracted from avatar base when comparing collision height", 0.1f ) , - new ParameterDefn("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)", + new ParameterDefn("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)", 2f ), - new ParameterDefn("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step", + new ParameterDefn("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step", 0f ), - new ParameterDefn("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step", + new ParameterDefn("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step", 0.8f ), - new ParameterDefn("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs", + new ParameterDefn("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs", 1 ), new ParameterDefn("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle", @@ -688,130 +704,136 @@ public static class BSParam new ParameterDefn("VehicleEnableAngularBanking", "Turn on/off vehicle angular banking effect", true ), - new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)", + new ParameterDefn("MaxPersistantManifoldPoolSize", "Number of manifolds pooled (0 means default of 4096)", 0f, (s) => { return MaxPersistantManifoldPoolSize; }, (s,v) => { MaxPersistantManifoldPoolSize = v; s.UnmanagedParams[0].maxPersistantManifoldPoolSize = v; } ), - new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)", + new ParameterDefn("MaxCollisionAlgorithmPoolSize", "Number of collisions pooled (0 means default of 4096)", 0f, (s) => { return MaxCollisionAlgorithmPoolSize; }, (s,v) => { MaxCollisionAlgorithmPoolSize = v; s.UnmanagedParams[0].maxCollisionAlgorithmPoolSize = v; } ), - new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count", + new ParameterDefn("ShouldDisableContactPoolDynamicAllocation", "Enable to allow large changes in object count", false, (s) => { return ShouldDisableContactPoolDynamicAllocation; }, (s,v) => { ShouldDisableContactPoolDynamicAllocation = v; s.UnmanagedParams[0].shouldDisableContactPoolDynamicAllocation = NumericBool(v); } ), - new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step", + new ParameterDefn("ShouldForceUpdateAllAabbs", "Enable to recomputer AABBs every simulator step", false, (s) => { return ShouldForceUpdateAllAabbs; }, (s,v) => { ShouldForceUpdateAllAabbs = v; s.UnmanagedParams[0].shouldForceUpdateAllAabbs = NumericBool(v); } ), - new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction", + new ParameterDefn("ShouldRandomizeSolverOrder", "Enable for slightly better stacking interaction", true, (s) => { return ShouldRandomizeSolverOrder; }, (s,v) => { ShouldRandomizeSolverOrder = v; s.UnmanagedParams[0].shouldRandomizeSolverOrder = NumericBool(v); } ), - new ParameterDefn("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands", + new ParameterDefn("ShouldSplitSimulationIslands", "Enable splitting active object scanning islands", true, (s) => { return ShouldSplitSimulationIslands; }, (s,v) => { ShouldSplitSimulationIslands = v; s.UnmanagedParams[0].shouldSplitSimulationIslands = NumericBool(v); } ), - new ParameterDefn("ShouldEnableFrictionCaching", "Enable friction computation caching", + new ParameterDefn("ShouldEnableFrictionCaching", "Enable friction computation caching", true, (s) => { return ShouldEnableFrictionCaching; }, (s,v) => { ShouldEnableFrictionCaching = v; s.UnmanagedParams[0].shouldEnableFrictionCaching = NumericBool(v); } ), - new ParameterDefn("NumberOfSolverIterations", "Number of internal iterations (0 means default)", + new ParameterDefn("NumberOfSolverIterations", "Number of internal iterations (0 means default)", 0f, // zero says use Bullet default (s) => { return NumberOfSolverIterations; }, (s,v) => { NumberOfSolverIterations = v; s.UnmanagedParams[0].numberOfSolverIterations = v; } ), - new ParameterDefn("UseSingleSidedMeshes", "Whether to compute collisions based on single sided meshes.", + new ParameterDefn("UseSingleSidedMeshes", "Whether to compute collisions based on single sided meshes.", true, (s) => { return UseSingleSidedMeshes; }, (s,v) => { UseSingleSidedMeshes = v; s.UnmanagedParams[0].useSingleSidedMeshes = NumericBool(v); } ), - new ParameterDefn("GlobalContactBreakingThreshold", "Amount of shape radius before breaking a collision contact (0 says Bullet default (0.2))", + new ParameterDefn("GlobalContactBreakingThreshold", "Amount of shape radius before breaking a collision contact (0 says Bullet default (0.2))", 0f, (s) => { return GlobalContactBreakingThreshold; }, (s,v) => { GlobalContactBreakingThreshold = v; s.UnmanagedParams[0].globalContactBreakingThreshold = v; } ), - new ParameterDefn("PhysicsUnmanLoggingFrames", "If non-zero, frames between output of detailed unmanaged physics statistics", + new ParameterDefn("PhysicsUnmanLoggingFrames", "If non-zero, frames between output of detailed unmanaged physics statistics", 0f, (s) => { return PhysicsUnmanLoggingFrames; }, (s,v) => { PhysicsUnmanLoggingFrames = v; s.UnmanagedParams[0].physicsLoggingFrames = v; } ), - new ParameterDefn("CSHullMaxDepthSplit", "CS impl: max depth to split for hull. 1-10 but > 7 is iffy", + new ParameterDefn("CSHullMaxDepthSplit", "CS impl: max depth to split for hull. 1-10 but > 7 is iffy", 7 ), - new ParameterDefn("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes", + new ParameterDefn("CSHullMaxDepthSplitForSimpleShapes", "CS impl: max depth setting for simple prim shapes", 2 ), - new ParameterDefn("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)", + new ParameterDefn("CSHullConcavityThresholdPercent", "CS impl: concavity threshold percent (0-20)", 5f ), - new ParameterDefn("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)", + new ParameterDefn("CSHullVolumeConservationThresholdPercent", "percent volume conservation to collapse hulls (0-30)", 5f ), - new ParameterDefn("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.", + new ParameterDefn("CSHullMaxVertices", "CS impl: maximum number of vertices in output hulls. Keep < 50.", 32 ), - new ParameterDefn("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.", + new ParameterDefn("CSHullMaxSkinWidth", "CS impl: skin width to apply to output hulls.", 0f ), - new ParameterDefn("BHullMaxVerticesPerHull", "Bullet impl: max number of vertices per created hull", + new ParameterDefn("BHullMaxVerticesPerHull", "Bullet impl: max number of vertices per created hull", 200f ), - new ParameterDefn("BHullMinClusters", "Bullet impl: minimum number of hulls to create per mesh", + new ParameterDefn("BHullMinClusters", "Bullet impl: minimum number of hulls to create per mesh", 10f ), - new ParameterDefn("BHullCompacityWeight", "Bullet impl: weight factor for how compact to make hulls", + new ParameterDefn("BHullCompacityWeight", "Bullet impl: weight factor for how compact to make hulls", 20f ), - new ParameterDefn("BHullVolumeWeight", "Bullet impl: weight factor for volume in created hull", + new ParameterDefn("BHullVolumeWeight", "Bullet impl: weight factor for volume in created hull", 0.1f ), - new ParameterDefn("BHullConcavity", "Bullet impl: weight factor for how convex a created hull can be", + new ParameterDefn("BHullConcavity", "Bullet impl: weight factor for how convex a created hull can be", 10f ), - new ParameterDefn("BHullAddExtraDistPoints", "Bullet impl: whether to add extra vertices for long distance vectors", + new ParameterDefn("BHullAddExtraDistPoints", "Bullet impl: whether to add extra vertices for long distance vectors", true ), - new ParameterDefn("BHullAddNeighboursDistPoints", "Bullet impl: whether to add extra vertices between neighbor hulls", + new ParameterDefn("BHullAddNeighboursDistPoints", "Bullet impl: whether to add extra vertices between neighbor hulls", true ), - new ParameterDefn("BHullAddFacesPoints", "Bullet impl: whether to add extra vertices to break up hull faces", + new ParameterDefn("BHullAddFacesPoints", "Bullet impl: whether to add extra vertices to break up hull faces", true ), - new ParameterDefn("BHullShouldAdjustCollisionMargin", "Bullet impl: whether to shrink resulting hulls to account for collision margin", + new ParameterDefn("BHullShouldAdjustCollisionMargin", "Bullet impl: whether to shrink resulting hulls to account for collision margin", false ), - new ParameterDefn("WhichHACD", "zero if Bullet HACD, non-zero says VHACD", + new ParameterDefn("WhichHACD", "zero if Bullet HACD, non-zero says VHACD", 0f ), - new ParameterDefn("VHACDresolution", "max number of voxels generated during voxelization stage", + new ParameterDefn("VHACDresolution", "max number of voxels generated during voxelization stage", 100000f ), - new ParameterDefn("VHACDdepth", "max number of clipping stages", + new ParameterDefn("VHACDdepth", "max number of clipping stages", 20f ), - new ParameterDefn("VHACDconcavity", "maximum concavity", + new ParameterDefn("VHACDconcavity", "maximum concavity", 0.0025f ), - new ParameterDefn("VHACDplaneDownsampling", "granularity of search for best clipping plane", + new ParameterDefn("VHACDplaneDownsampling", "granularity of search for best clipping plane", 4f ), - new ParameterDefn("VHACDconvexHullDownsampling", "precision of hull gen process", + new ParameterDefn("VHACDconvexHullDownsampling", "precision of hull gen process", 4f ), - new ParameterDefn("VHACDalpha", "bias toward clipping along symmetry planes", + new ParameterDefn("VHACDalpha", "bias toward clipping along symmetry planes", 0.05f ), - new ParameterDefn("VHACDbeta", "bias toward clipping along revolution axis", + new ParameterDefn("VHACDbeta", "bias toward clipping along revolution axis", 0.05f ), - new ParameterDefn("VHACDgamma", "max concavity when merging", + new ParameterDefn("VHACDgamma", "max concavity when merging", 0.00125f ), - new ParameterDefn("VHACDpca", "on/off normalizing mesh before decomp", + new ParameterDefn("VHACDpca", "on/off normalizing mesh before decomp", 0f ), - new ParameterDefn("VHACDmode", "0:voxel based, 1: tetrahedron based", + new ParameterDefn("VHACDmode", "0:voxel based, 1: tetrahedron based", 0f ), - new ParameterDefn("VHACDmaxNumVerticesPerCH", "max triangles per convex hull", + new ParameterDefn("VHACDmaxNumVerticesPerCH", "max triangles per convex hull", 64f ), - new ParameterDefn("VHACDminVolumePerCH", "sampling of generated convex hulls", + new ParameterDefn("VHACDminVolumePerCH", "sampling of generated convex hulls", 0.0001f ), - new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)", + new ParameterDefn("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)", (float)BSLinkset.LinksetImplementation.Compound ), - new ParameterDefn("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same", + new ParameterDefn("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same", true ), - new ParameterDefn("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.", + new ParameterDefn("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.", false ), - new ParameterDefn("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints", + new ParameterDefn("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints", true ), - new ParameterDefn("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints", + new ParameterDefn("LinkConstraintTransMotorMaxVel", "Maximum velocity to be applied by translational motor in linkset constraints", 5.0f ), - new ParameterDefn("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints", + new ParameterDefn("LinkConstraintTransMotorMaxForce", "Maximum force to be applied by translational motor in linkset constraints", 0.1f ), - new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1", + new ParameterDefn("LinkConstraintCFM", "Amount constraint can be violated. 0=no violation, 1=infinite. Default=0.1", 0.1f ), - new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2", + new ParameterDefn("LinkConstraintERP", "Amount constraint is corrected each tick. 0=none, 1=all. Default = 0.2", 0.1f ), - new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)", + new ParameterDefn("LinkConstraintSolverIterations", "Number of solver iterations when computing constraint. (0 = Bullet default)", 40 ), + new ParameterDefn("UseBulletRaycast", "If 'true', use the raycast function of the Bullet physics engine", + true ), + + new ParameterDefn("DebugNumber", "A console setable number sometimes used for debugging", + 1.0f ), + new ParameterDefn("PhysicsMetricFrames", "Frames between outputting detailed phys metrics. (0 is off)", 0, (s) => { return s.PhysicsMetricDumpFrames; }, @@ -819,7 +841,7 @@ public static class BSParam new ParameterDefn("ResetBroadphasePool", "Setting this is any value resets the broadphase collision pool", 0f, (s) => { return 0f; }, - (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v, false /* inTaintTime */); } ), + (s,v) => { BSParam.ResetBroadphasePoolTainted(s, v); } ), new ParameterDefn("ResetConstraintSolver", "Setting this is any value resets the constraint solver", 0f, (s) => { return 0f; }, @@ -905,10 +927,10 @@ public static class BSParam // ===================================================================== // There are parameters that, when set, cause things to happen in the physics engine. // This causes the broadphase collision cache to be cleared. - private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v, bool inTaintTime) + private static void ResetBroadphasePoolTainted(BSScene pPhysScene, float v) { BSScene physScene = pPhysScene; - physScene.TaintedObject(inTaintTime, "BSParam.ResetBroadphasePoolTainted", delegate() + physScene.TaintedObject(BSScene.DetailLogZero, "BSParam.ResetBroadphasePoolTainted", delegate() { physScene.PE.ResetBroadphasePool(physScene.World); }); diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs old mode 100644 new mode 100755 index da3fc18..6aa24d5 --- a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs @@ -230,34 +230,97 @@ public abstract class BSPhysObject : PhysicsActor // Update the physical location and motion of the object. Called with data from Bullet. public abstract void UpdateProperties(EntityProperties entprop); + // The position value as known by BulletSim. Does not effect the physics engine. public virtual OMV.Vector3 RawPosition { get; set; } + // Set position in BulletSim and the physics engined to a value immediately. Must be called at taint time. public abstract OMV.Vector3 ForcePosition { get; set; } + // The orientation value as known by BulletSim. Does not effect the physics engine. public virtual OMV.Quaternion RawOrientation { get; set; } + // Set orientation in BulletSim and the physics engine to a value immediately. Must be called at taint time. public abstract OMV.Quaternion ForceOrientation { get; set; } + // The velocity value as known by BulletSim. Does not effect the physics engine. public virtual OMV.Vector3 RawVelocity { get; set; } + // Set velocity in BulletSim and the physics engined to a value immediately. Must be called at taint time. public abstract OMV.Vector3 ForceVelocity { get; set; } + // The rotational velocity value as known by BulletSim. Does not effect the physics engine. + public OMV.Vector3 RawRotationalVelocity { get; set; } + + // RawForce is a constant force applied to object (see Force { set; } ) public OMV.Vector3 RawForce { get; set; } public OMV.Vector3 RawTorque { get; set; } + public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { - AddAngularForce(force, pushforce, false); + AddAngularForce(false, force); } - public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); - public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime); + public abstract void AddAngularForce(bool inTaintTime, OMV.Vector3 force); + public abstract void AddForce(bool inTaintTime, OMV.Vector3 force); - public abstract OMV.Vector3 ForceRotationalVelocity { get; set; } + // PhysicsActor.Velocity + public override OMV.Vector3 Velocity + { + get { return RawVelocity; } + set + { + // This sets the velocity now. BSCharacter will override to clear target velocity + // before calling this. + RawVelocity = value; + PhysScene.TaintedObject(LocalID, TypeName + ".SetVelocity", delegate () { + // DetailLog("{0},BSPhysObject.Velocity.set,vel={1}", LocalID, RawVelocity); + ForceVelocity = RawVelocity; + }); + } + } + + // PhysicsActor.SetMomentum + // All the physics engines use this as a way of forcing the velocity to something. + // BSCharacter overrides this so it can set the target velocity to zero before calling this. + public override void SetMomentum(OMV.Vector3 momentum) + { + this.Velocity = momentum; + } + + public override OMV.Vector3 RotationalVelocity { + get { + return RawRotationalVelocity; + } + set { + RawRotationalVelocity = value; + Util.ClampV(RawRotationalVelocity, BSParam.MaxAngularVelocity); + // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); + PhysScene.TaintedObject(LocalID, TypeName + ".setRotationalVelocity", delegate() + { + ForceRotationalVelocity = RawRotationalVelocity; + }); + } + } + public OMV.Vector3 ForceRotationalVelocity { + get { + return RawRotationalVelocity; + } + set { + RawRotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity); + if (PhysBody.HasPhysicalBody) + { + DetailLog("{0},{1}.ForceRotationalVel,taint,rotvel={2}", LocalID, TypeName, RawRotationalVelocity); + PhysScene.PE.SetAngularVelocity(PhysBody, RawRotationalVelocity); + // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity); + ActivateIfPhysical(false); + } + } + } public abstract float ForceBuoyancy { get; set; } public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; } - public override bool PIDActive + public override bool PIDActive { get { return MoveToTargetActive; } - set { MoveToTargetActive = value; } + set { MoveToTargetActive = value; } } public override OMV.Vector3 PIDTarget { set { MoveToTargetTarget = value; } } @@ -268,7 +331,7 @@ public abstract class BSPhysObject : PhysicsActor public float MoveToTargetTau { get; set; } // Used for llSetHoverHeight and maybe vehicle height. Hover Height will override MoveTo target's Z - public override bool PIDHoverActive { set { HoverActive = value; } } + public override bool PIDHoverActive {get {return HoverActive;} set { HoverActive = value; } } public override float PIDHoverHeight { set { HoverHeight = value; } } public override PIDHoverType PIDHoverType { set { HoverType = value; } } public override float PIDHoverTau { set { HoverTau = value; } } @@ -452,18 +515,24 @@ public abstract class BSPhysObject : PhysicsActor private long CollisionsLastTickStep = -1; // The simulation step is telling this object about a collision. + // I'm the 'collider', the thing I'm colliding with is the 'collidee'. // Return 'true' if a collision was processed and should be sent up. // Return 'false' if this object is not enabled/subscribed/appropriate for or has already seen this collision. // Called at taint time from within the Step() function - public delegate bool CollideCall(uint collidingWith, BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth); - public virtual bool Collide(uint collidingWith, BSPhysObject collidee, - OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) + public virtual bool Collide(BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) { bool ret = false; + // if 'collidee' is null, that means it is terrain + uint collideeLocalID = (collidee == null) ? BSScene.TERRAIN_ID : collidee.LocalID; + // All terrain goes by the TERRAIN_ID id when passed up as a collision + if (collideeLocalID <= PhysScene.TerrainManager.HighestTerrainID) { + collideeLocalID = BSScene.TERRAIN_ID; + } + // The following lines make IsColliding(), CollidingGround() and CollidingObj work CollidingStep = PhysScene.SimulationStep; - if (collidingWith <= PhysScene.TerrainManager.HighestTerrainID) + if (collideeLocalID == BSScene.TERRAIN_ID) { CollidingGroundStep = PhysScene.SimulationStep; } @@ -474,10 +543,13 @@ public abstract class BSPhysObject : PhysicsActor CollisionAccumulation++; - // For movement tests, remember if we are colliding with an object that is moving. - ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero) : false; + // For movement tests, if the collider is me, remember if we are colliding with an object that is moving. + // Here the 'collider'/'collidee' thing gets messed up. In the larger context, when something is checking + // if the thing it is colliding with is moving, for instance, it asks if the its collider is moving. + ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero || collidee.RotationalVelocity != OMV.Vector3.Zero) : false; ColliderIsVolumeDetect = collidee != null ? (collidee.IsVolumeDetect) : false; + // Make a collection of the collisions that happened the last simulation tick. // This is different than the collection created for sending up to the simulator as it is cleared every tick. if (CollisionsLastTickStep != PhysScene.SimulationStep) @@ -485,16 +557,29 @@ public abstract class BSPhysObject : PhysicsActor CollisionsLastTick = new CollisionEventUpdate(); CollisionsLastTickStep = PhysScene.SimulationStep; } - CollisionsLastTick.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); + CollisionsLastTick.AddCollider(collideeLocalID, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); // If someone has subscribed for collision events log the collision so it will be reported up if (SubscribedEvents()) { + ContactPoint newContact = new ContactPoint(contactPoint, contactNormal, pentrationDepth); + + // Collision sound requires a velocity to know it should happen. This is a lot of computation for a little used feature. + OMV.Vector3 relvel = OMV.Vector3.Zero; + if (IsPhysical) + relvel = RawVelocity; + if (collidee != null && collidee.IsPhysical) + relvel -= collidee.RawVelocity; + newContact.RelativeSpeed = -OMV.Vector3.Dot(relvel, contactNormal); + // DetailLog("{0},{1}.Collision.AddCollider,vel={2},contee.vel={3},relvel={4},relspeed={5}", + // LocalID, TypeName, RawVelocity, (collidee == null ? OMV.Vector3.Zero : collidee.RawVelocity), relvel, newContact.RelativeSpeed); + lock (PhysScene.CollisionLock) { - CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); + CollisionCollection.AddCollider(collideeLocalID, newContact); } - DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},colliderMoving={6}", - LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth, ColliderIsMoving); + DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},speed={6},colliderMoving={7}", + LocalID, TypeName, collideeLocalID, contactPoint, contactNormal, pentrationDepth, + newContact.RelativeSpeed, ColliderIsMoving); ret = true; } @@ -555,7 +640,11 @@ public abstract class BSPhysObject : PhysicsActor PhysScene.TaintedObject(LocalID, TypeName+".SubscribeEvents", delegate() { if (PhysBody.HasPhysicalBody) + { CurrentCollisionFlags = PhysScene.PE.AddToCollisionFlags(PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS); + DetailLog("{0},{1}.SubscribeEvents,setting collision. ms={2}, collisionFlags={3:x}", + LocalID, TypeName, SubscribedEventsMs, CurrentCollisionFlags); + } }); } else diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs index 6f27ac7..f085d70 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs @@ -33,7 +33,7 @@ using log4net; using OMV = OpenMetaverse; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet; +using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet; namespace OpenSim.Region.PhysicsModule.BulletS { @@ -59,7 +59,6 @@ public class BSPrim : BSPhysObject private bool _setAlwaysRun; private bool _throttleUpdates; private bool _floatOnWater; - private OMV.Vector3 _rotationalVelocity; private bool _kinematic; private float _buoyancy; @@ -90,7 +89,7 @@ public class BSPrim : BSPhysObject RawOrientation = rotation; _buoyancy = 0f; RawVelocity = OMV.Vector3.Zero; - _rotationalVelocity = OMV.Vector3.Zero; + RawRotationalVelocity = OMV.Vector3.Zero; BaseShape = pbs; _isPhysical = pisPhysical; _isVolumeDetect = false; @@ -256,7 +255,7 @@ public class BSPrim : BSPhysObject { RawVelocity = OMV.Vector3.Zero; _acceleration = OMV.Vector3.Zero; - _rotationalVelocity = OMV.Vector3.Zero; + RawRotationalVelocity = OMV.Vector3.Zero; // Zero some other properties in the physics engine PhysScene.TaintedObject(inTaintTime, LocalID, "BSPrim.ZeroMotion", delegate() @@ -267,33 +266,33 @@ public class BSPrim : BSPhysObject } public override void ZeroAngularMotion(bool inTaintTime) { - _rotationalVelocity = OMV.Vector3.Zero; + RawRotationalVelocity = OMV.Vector3.Zero; // Zero some other properties in the physics engine PhysScene.TaintedObject(inTaintTime, LocalID, "BSPrim.ZeroMotion", delegate() { // DetailLog("{0},BSPrim.ZeroAngularMotion,call,rotVel={1}", LocalID, _rotationalVelocity); if (PhysBody.HasPhysicalBody) { - PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity); - PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity); + PhysScene.PE.SetInterpolationAngularVelocity(PhysBody, RawRotationalVelocity); + PhysScene.PE.SetAngularVelocity(PhysBody, RawRotationalVelocity); } }); } - public override void LockAngularMotion(OMV.Vector3 axis) + public override void LockAngularMotion(byte axislocks) { - DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis); + DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axislocks); ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_UNLOCK_ANGULAR, 0f, 0f); - if (axis.X != 1) + if ((axislocks & 0x02) != 0) { ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_X, 0f, 0f); } - if (axis.Y != 1) + if ((axislocks & 0x04) != 0) { ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_Y, 0f, 0f); } - if (axis.Z != 1) + if ((axislocks & 0x08) != 0) { ApplyAxisLimits(ExtendedPhysics.PHYS_AXIS_LOCK_ANGULAR_Z, 0f, 0f); } @@ -394,7 +393,7 @@ public class BSPrim : BSPhysObject // Apply upforce and overcome gravity. OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity; DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, RawPosition, upForce, correctionForce); - AddForce(correctionForce, false, inTaintTime); + AddForce(inTaintTime, correctionForce); ret = true; } } @@ -426,9 +425,9 @@ public class BSPrim : BSPhysObject RawVelocity = Util.ClampV(RawVelocity, BSParam.MaxLinearVelocity); ret = true; } - if (_rotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared) + if (RawRotationalVelocity.LengthSquared() > BSParam.MaxAngularVelocitySquared) { - _rotationalVelocity = Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity); + RawRotationalVelocity = Util.ClampV(RawRotationalVelocity, BSParam.MaxAngularVelocity); ret = true; } @@ -647,6 +646,59 @@ public class BSPrim : BSPhysObject }); } + public override void SetVehicle(object pvdata) + { + PhysScene.TaintedObject(LocalID, "BSPrim.SetVehicle", delegate () + { + BSDynamics vehicleActor = GetVehicleActor(true /* createIfNone */); + if (vehicleActor != null && (pvdata is VehicleData) ) + { + VehicleData vdata = (VehicleData)pvdata; + // vehicleActor.ProcessSetVehicle((VehicleData)vdata); + + vehicleActor.ProcessTypeChange(vdata.m_type); + vehicleActor.ProcessVehicleFlags(-1, false); + vehicleActor.ProcessVehicleFlags((int)vdata.m_flags, false); + + // Linear properties + vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_MOTOR_DIRECTION, vdata.m_linearMotorDirection); + vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_FRICTION_TIMESCALE, vdata.m_linearFrictionTimescale); + vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, vdata.m_linearMotorDecayTimescale); + vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_MOTOR_TIMESCALE, vdata.m_linearMotorTimescale); + vehicleActor.ProcessVectorVehicleParam(Vehicle.LINEAR_MOTOR_OFFSET, vdata.m_linearMotorOffset); + + //Angular properties + vehicleActor.ProcessVectorVehicleParam(Vehicle.ANGULAR_MOTOR_DIRECTION, vdata.m_angularMotorDirection); + vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_MOTOR_TIMESCALE, vdata.m_angularMotorTimescale); + vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, vdata.m_angularMotorDecayTimescale); + vehicleActor.ProcessVectorVehicleParam(Vehicle.ANGULAR_FRICTION_TIMESCALE, vdata.m_angularFrictionTimescale); + + //Deflection properties + vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, vdata.m_angularDeflectionEfficiency); + vehicleActor.ProcessFloatVehicleParam(Vehicle.ANGULAR_DEFLECTION_TIMESCALE, vdata.m_angularDeflectionTimescale); + vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_DEFLECTION_EFFICIENCY, vdata.m_linearDeflectionEfficiency); + vehicleActor.ProcessFloatVehicleParam(Vehicle.LINEAR_DEFLECTION_TIMESCALE, vdata.m_linearDeflectionTimescale); + + //Banking properties + vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_EFFICIENCY, vdata.m_bankingEfficiency); + vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_MIX, vdata.m_bankingMix); + vehicleActor.ProcessFloatVehicleParam(Vehicle.BANKING_TIMESCALE, vdata.m_bankingTimescale); + + //Hover and Buoyancy properties + vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_HEIGHT, vdata.m_VhoverHeight); + vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_EFFICIENCY, vdata.m_VhoverEfficiency); + vehicleActor.ProcessFloatVehicleParam(Vehicle.HOVER_TIMESCALE, vdata.m_VhoverTimescale); + vehicleActor.ProcessFloatVehicleParam(Vehicle.BUOYANCY, vdata.m_VehicleBuoyancy); + + //Attractor properties + vehicleActor.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, vdata.m_verticalAttractionEfficiency); + vehicleActor.ProcessFloatVehicleParam(Vehicle.VERTICAL_ATTRACTION_TIMESCALE, vdata.m_verticalAttractionTimescale); + + vehicleActor.ProcessRotationVehicleParam(Vehicle.REFERENCE_FRAME, vdata.m_referenceFrame); + } + }); + } + // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more public override void SetVolumeDetect(int param) { bool newValue = (param != 0); @@ -735,22 +787,9 @@ public class BSPrim : BSPhysObject } } } - public override OMV.Vector3 Velocity { - get { return RawVelocity; } - set { - RawVelocity = value; - PhysScene.TaintedObject(LocalID, "BSPrim.setVelocity", delegate() - { - // DetailLog("{0},BSPrim.SetVelocity,taint,vel={1}", LocalID, RawVelocity); - ForceVelocity = RawVelocity; - }); - } - } public override OMV.Vector3 ForceVelocity { get { return RawVelocity; } set { - PhysScene.AssertInTaintTime("BSPrim.ForceVelocity"); - RawVelocity = Util.ClampV(value, BSParam.MaxLinearVelocity); if (PhysBody.HasPhysicalBody) { @@ -955,7 +994,7 @@ public class BSPrim : BSPhysObject // For good measure, make sure the transform is set through to the motion state ForcePosition = RawPosition; ForceVelocity = RawVelocity; - ForceRotationalVelocity = _rotationalVelocity; + ForceRotationalVelocity = RawRotationalVelocity; // A dynamic object has mass UpdatePhysicalMassProperties(RawMass, false); @@ -1075,35 +1114,6 @@ public class BSPrim : BSPhysObject }); } } - public override OMV.Vector3 RotationalVelocity { - get { - return _rotationalVelocity; - } - set { - _rotationalVelocity = value; - Util.ClampV(_rotationalVelocity, BSParam.MaxAngularVelocity); - // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); - PhysScene.TaintedObject(LocalID, "BSPrim.setRotationalVelocity", delegate() - { - ForceRotationalVelocity = _rotationalVelocity; - }); - } - } - public override OMV.Vector3 ForceRotationalVelocity { - get { - return _rotationalVelocity; - } - set { - _rotationalVelocity = Util.ClampV(value, BSParam.MaxAngularVelocity); - if (PhysBody.HasPhysicalBody) - { - DetailLog("{0},BSPrim.ForceRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity); - PhysScene.PE.SetAngularVelocity(PhysBody, _rotationalVelocity); - // PhysicsScene.PE.SetInterpolationAngularVelocity(PhysBody, _rotationalVelocity); - ActivateIfPhysical(false); - } - } - } public override bool Kinematic { get { return _kinematic; } set { _kinematic = value; @@ -1132,14 +1142,14 @@ public class BSPrim : BSPhysObject } } - public override bool PIDActive + public override bool PIDActive { get { return MoveToTargetActive; } - set + set { MoveToTargetActive = value; @@ -1167,12 +1177,16 @@ public class BSPrim : BSPhysObject // if the actor exists, tell it to refresh its values. actor.Refresh(); } - + } } // Used for llSetHoverHeight and maybe vehicle height // Hover Height will override MoveTo target's Z public override bool PIDHoverActive { + get + { + return base.HoverActive; + } set { base.HoverActive = value; EnableActor(HoverActive, HoverActorName, delegate() @@ -1192,14 +1206,18 @@ public class BSPrim : BSPhysObject // Per documentation, max force is limited. OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude); - // Since this force is being applied in only one step, make this a force per second. - addForce /= PhysScene.LastTimeStep; - AddForce(addForce, pushforce, false /* inTaintTime */); + // Push forces seem to be scaled differently (follow pattern in ubODE) + if (!pushforce) { + // Since this force is being applied in only one step, make this a force per second. + addForce /= PhysScene.LastTimeStep; + } + + AddForce(false /* inTaintTime */, addForce); } // Applying a force just adds this to the total force on the object. // This added force will only last the next simulation tick. - public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) { + public override void AddForce(bool inTaintTime, OMV.Vector3 force) { // for an object, doesn't matter if force is a pushforce or not if (IsPhysicallyActive) { @@ -1258,7 +1276,7 @@ public class BSPrim : BSPhysObject } // BSPhysObject.AddAngularForce() - public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) + public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force) { if (force.IsFinite()) { @@ -1297,9 +1315,6 @@ public class BSPrim : BSPhysObject }); } - public override void SetMomentum(OMV.Vector3 momentum) { - // DetailLog("{0},BSPrim.SetMomentum,call,mom={1}", LocalID, momentum); - } #region Mass Calculation private float CalculateMass() @@ -1869,7 +1884,7 @@ public class BSPrim : BSPhysObject if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(RawVelocity, BSParam.UpdateVelocityChangeThreshold)) RawVelocity = entprop.Velocity; _acceleration = entprop.Acceleration; - _rotationalVelocity = entprop.RotationalVelocity; + RawRotationalVelocity = entprop.RotationalVelocity; // DetailLog("{0},BSPrim.UpdateProperties,afterAssign,entprop={1}", LocalID, entprop); // DEBUG DEBUG @@ -1878,7 +1893,7 @@ public class BSPrim : BSPhysObject { entprop.Position = RawPosition; entprop.Velocity = RawVelocity; - entprop.RotationalVelocity = _rotationalVelocity; + entprop.RotationalVelocity = RawRotationalVelocity; entprop.Acceleration = _acceleration; } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs old mode 100644 new mode 100755 index d8ed56b..3f90fc5 --- a/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrimDisplaced.cs @@ -81,7 +81,6 @@ public class BSPrimDisplaced : BSPrim // Called at taint time. public virtual Vector3 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement) { - PhysScene.AssertInTaintTime("BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement"); Vector3 comDisp; if (UserSetCenterOfMassDisplacement.HasValue) comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement; diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs old mode 100644 new mode 100755 index 55b5da0..563dcfa --- a/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrimLinkable.cs @@ -203,15 +203,14 @@ public class BSPrimLinkable : BSPrimDisplaced // Called after a simulation step to post a collision with this object. // This returns 'true' if the collision has been queued and the SendCollisions call must // be made at the end of the simulation step. - public override bool Collide(uint collidingWith, BSPhysObject collidee, - OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) + public override bool Collide(BSPhysObject collidee, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) { bool ret = false; // Ask the linkset if it wants to handle the collision - if (!Linkset.HandleCollide(collidingWith, collidee, contactPoint, contactNormal, pentrationDepth)) + if (!Linkset.HandleCollide(this, collidee, contactPoint, contactNormal, pentrationDepth)) { // The linkset didn't handle it so pass the collision through normal processing - ret = base.Collide(collidingWith, collidee, contactPoint, contactNormal, pentrationDepth); + ret = base.Collide(collidee, contactPoint, contactNormal, pentrationDepth); } return ret; } diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs index 452ce55..163efaa 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSScene.cs @@ -124,9 +124,10 @@ namespace OpenSim.Region.PhysicsModule.BulletS // True if initialized and ready to do simulation steps private bool m_initialized = false; - // Flag which is true when processing taints. - // Not guaranteed to be correct all the time (don't depend on this) but good for debugging. - public bool InTaintTime { get; private set; } + // Object locked whenever execution is inside the physics engine + public Object PhysicsEngineLock = new object(); + // Flag that is true when the simulator is active and shouldn't be touched + public bool InSimulationTime { get; private set; } // Pinned memory used to pass step information between managed and unmanaged internal int m_maxCollisionsPerFrame; @@ -212,6 +213,11 @@ namespace OpenSim.Region.PhysicsModule.BulletS get { return "BulletSim"; } } + public string Version + { + get { return "1.0"; } + } + public Type ReplaceableInterface { get { return null; } @@ -245,6 +251,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS EngineType = Name; RegionName = scene.RegionInfo.RegionName; PhysicsSceneName = EngineType + "/" + RegionName; + EngineName = Name + " " + Version; scene.RegisterModuleInterface(this); Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ); @@ -338,19 +345,19 @@ namespace OpenSim.Region.PhysicsModule.BulletS // Put some informational messages into the log file. m_log.InfoFormat("{0} Linksets implemented with {1}", LogHeader, (BSLinkset.LinksetImplementation)BSParam.LinksetImplementation); - InTaintTime = false; + InSimulationTime = false; m_initialized = true; // If the physics engine runs on its own thread, start same. if (BSParam.UseSeparatePhysicsThread) { // The physics simulation should happen independently of the heartbeat loop - m_physicsThread + m_physicsThread = WorkManager.StartThread( - BulletSPluginPhysicsThread, - string.Format("{0} ({1})", BulletEngineName, RegionName), - ThreadPriority.Normal, - true, + BulletSPluginPhysicsThread, + string.Format("{0} ({1})", BulletEngineName, RegionName), + ThreadPriority.Normal, + true, true); } } @@ -523,13 +530,13 @@ namespace OpenSim.Region.PhysicsModule.BulletS return null; } - public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) + public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float footOffset, bool isFlying) { // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName); if (!m_initialized) return null; - BSCharacter actor = new BSCharacter(localID, avName, this, position, velocity, size, isFlying); + BSCharacter actor = new BSCharacter(localID, avName, this, position, Vector3.Zero, size, footOffset, isFlying); lock (PhysObjects) PhysObjects.Add(localID, actor); @@ -651,49 +658,57 @@ namespace OpenSim.Region.PhysicsModule.BulletS int beforeTime = Util.EnvironmentTickCount(); int simTime = 0; + int numTaints = 0; + int numSubSteps = 0; - int numTaints = _taintOperations.Count; - InTaintTime = true; // Only used for debugging so locking is not necessary. + lock (PhysicsEngineLock) + { + InSimulationTime = true; + // update the prim states while we know the physics engine is not busy + numTaints += ProcessTaints(); - // update the prim states while we know the physics engine is not busy - ProcessTaints(); + // Some of the physical objects requre individual, pre-step calls + // (vehicles and avatar movement, in particular) + TriggerPreStepEvent(timeStep); - // Some of the physical objects requre individual, pre-step calls - // (vehicles and avatar movement, in particular) - TriggerPreStepEvent(timeStep); + // the prestep actions might have added taints + numTaints += ProcessTaints(); - // the prestep actions might have added taints - numTaints += _taintOperations.Count; - ProcessTaints(); + // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world. + // Only enable this in a limited test world with few objects. + if (m_physicsPhysicalDumpEnabled) + PE.DumpAllInfo(World); - InTaintTime = false; // Only used for debugging so locking is not necessary. + // step the physical world one interval + m_simulationStep++; + try + { + numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount); + } + catch (Exception e) + { + m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}", + LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e); + DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}", + DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount); + updatedEntityCount = 0; + collidersCount = 0; + } - // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world. - // Only enable this in a limited test world with few objects. - if (m_physicsPhysicalDumpEnabled) - PE.DumpAllInfo(World); + // Make the physics engine dump useful statistics periodically + if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0)) + PE.DumpPhysicsStatistics(World); - // step the physical world one interval - m_simulationStep++; - int numSubSteps = 0; - try - { - numSubSteps = PE.PhysicsStep(World, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out collidersCount); + InSimulationTime = false; - } - catch (Exception e) - { - m_log.WarnFormat("{0},PhysicsStep Exception: nTaints={1}, substeps={2}, updates={3}, colliders={4}, e={5}", - LogHeader, numTaints, numSubSteps, updatedEntityCount, collidersCount, e); - DetailLog("{0},PhysicsStepException,call, nTaints={1}, substeps={2}, updates={3}, colliders={4}", - DetailLogZero, numTaints, numSubSteps, updatedEntityCount, collidersCount); - updatedEntityCount = 0; - collidersCount = 0; - } + // Some actors want to know when the simulation step is complete. + TriggerPostStepEvent(timeStep); + + // In case there were any parameter updates that happened during the simulation step + numTaints += ProcessTaints(); - // Make the physics engine dump useful statistics periodically - if (PhysicsMetricDumpFrames != 0 && ((m_simulationStep % PhysicsMetricDumpFrames) == 0)) - PE.DumpPhysicsStatistics(World); + InSimulationTime = false; + } // Get a value for 'now' so all the collision and update routines don't have to get their own. SimulationNowTime = Util.EnvironmentTickCount(); @@ -743,9 +758,6 @@ namespace OpenSim.Region.PhysicsModule.BulletS } } - // Some actors want to know when the simulation step is complete. - TriggerPostStepEvent(timeStep); - simTime = Util.EnvironmentTickCountSubtract(beforeTime); if (PhysicsLogging.Enabled) { @@ -762,7 +774,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS // The physics engine returns the number of milliseconds it simulated this call. // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS. // Multiply by a fixed nominal frame rate to give a rate similar to the simulator (usually 55). - m_simulatedTime += (float)numSubSteps * m_fixedTimeStep * 1000f * NominalFrameRate; +// m_simulatedTime += (float)numSubSteps * m_fixedTimeStep * 1000f * NominalFrameRate; + m_simulatedTime += (float)numSubSteps * m_fixedTimeStep; } // Called by a BSPhysObject to note that it has changed properties and this information @@ -849,7 +862,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS // Return the framerate simulated to give the above returned results. // (Race condition here but this is just bookkeeping so rare mistakes do not merit a lock). - float simTime = m_simulatedTime; + float simTime = m_simulatedTime / timeStep; m_simulatedTime = 0f; return simTime; } @@ -879,7 +892,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS if (collider.IsInitialized) { - if (collider.Collide(collidingWith, collidee, collidePoint, collideNormal, penetration)) + if (collider.Collide(collidee, collidePoint, collideNormal, penetration)) { // If a collision was 'good', remember to send it to the simulator lock (CollisionLock) @@ -948,26 +961,150 @@ namespace OpenSim.Region.PhysicsModule.BulletS // m_log.DebugFormat("{0}: DeleteTerrain()", LogHeader); } - // Although no one seems to check this, I do support combining. - public override bool SupportsCombining() + #endregion // Terrain + + #region Raycast + + public override bool SupportsRayCast() { - return TerrainManager.SupportsCombining(); + return BSParam.UseBulletRaycast; } - // This call says I am a child to region zero in a mega-region. 'pScene' is that - // of region zero, 'offset' is my offset from regions zero's origin, and - // 'extents' is the largest XY that is handled in my region. - public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) + + public override bool SupportsRaycastWorldFiltered() { - TerrainManager.Combine(pScene, offset, extents); + return BSParam.UseBulletRaycast; } - // Unhook all the combining that I know about. - public override void UnCombine(PhysicsScene pScene) + + /// + /// Queue a raycast against the physics scene. + /// The provided callback method will be called when the raycast is complete + /// + /// Many physics engines don't support collision testing at the same time as + /// manipulating the physics scene, so we queue the request up and callback + /// a custom method when the raycast is complete. + /// This allows physics engines that give an immediate result to callback immediately + /// and ones that don't, to callback when it gets a result back. + /// public delegate void RayCallback(List list); + /// + /// ODE for example will not allow you to change the scene while collision testing or + /// it asserts, 'opteration not valid for locked space'. This includes adding a ray to the scene. + /// + /// This is named RayCastWorld to not conflict with modrex's Raycast method. + /// + /// Origin of the ray + /// Direction of the ray + /// Length of ray in meters + /// Method to call when the raycast is complete + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) { - TerrainManager.UnCombine(pScene); + if (retMethod != null) + { + if (BSParam.UseBulletRaycast) + { + Vector3 posFrom = position; + Vector3 posTo = Vector3.Normalize(direction) * length + position; + + TaintedObject(DetailLogZero, "BSScene.RaycastWorld1", delegate () + { + RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, 0xffff, 0xffff); + retMethod(true, hitInfo.Point, hitInfo.ID, hitInfo.Fraction, hitInfo.Normal); + }); + } + else + { + retMethod(false, Vector3.Zero, 0, 999999999999f, Vector3.Zero); + } + } } - #endregion // Terrain + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + if (retMethod != null) + { + if (BSParam.UseBulletRaycast) + { + List hitInfo = RaycastWorld(position, direction, length, count); + retMethod(hitInfo); + } + else + { + retMethod(new List()); + } + } + } + + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int count) + { + return (List)RaycastWorld(position, direction, length, count, RayFilterFlags.All); + } + + public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int count, RayFilterFlags filter) + { + List ret = new List(); + if (BSParam.UseBulletRaycast) + { + uint collisionFilter = 0; + uint collisionMask = 0; + if ((filter & RayFilterFlags.land) != 0) + { + collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].group; + collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Terrain].mask; + } + if ((filter & RayFilterFlags.agent) != 0) + { + collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].group; + collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Avatar].mask; + } + if ((filter & RayFilterFlags.nonphysical) != 0) + { + collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Static].group; + collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Static].mask; + } + if ((filter & RayFilterFlags.physical) != 0) + { + collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].group; + collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.Dynamic].mask; + } + // if ((filter & RayFilterFlags.phantom) != 0) + // { + // collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group; + // collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask; + // } + if ((filter & RayFilterFlags.volumedtc) != 0) + { + collisionFilter |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].group; + collisionMask |= BulletSimData.CollisionTypeMasks[CollisionType.VolumeDetect].mask; + } + DetailLog("{0},RaycastWorld,pos={1},dir={2},len={3},count={4},filter={5},filter={6},mask={7}", + DetailLogZero, position, direction, length, count, filter, collisionFilter, collisionMask); + // NOTE: locking ensures the physics engine is not executing. + // The caller might have to wait for the physics engine to finish. + lock (PhysicsEngineLock) + { + Vector3 posFrom = position; + Vector3 posTo = Vector3.Normalize(direction) * length + position; + DetailLog("{0},RaycastWorld,RayTest2,from={1},to={2}", + DetailLogZero, posFrom, posTo); + RaycastHit hitInfo = PE.RayTest2(World, posFrom, posTo, collisionFilter, collisionMask); + if (hitInfo.hasHit()) + { + ContactResult result = new ContactResult(); + result.Pos = hitInfo.Point; + result.Normal = hitInfo.Normal; + result.ConsumerID = hitInfo.ID; + result.Depth = hitInfo.Fraction; + ret.Add(result); + DetailLog("{0},RaycastWorld,hit,pos={1},norm={2},depth={3},id={4}", + DetailLogZero, result.Pos, result.Normal, result.Depth, result.ConsumerID); + } + } + } + return ret; + } + + #endregion Raycast + public override Dictionary GetTopColliders() { @@ -1081,32 +1218,35 @@ namespace OpenSim.Region.PhysicsModule.BulletS // Calls to the PhysicsActors can't directly call into the physics engine // because it might be busy. We delay changes to a known time. // We rely on C#'s closure to save and restore the context for the delegate. - public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback) + // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed. + // public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback) + // { + // TaintedObject(BSScene.DetailLogZero, pIdent, pCallback); + // } + // NOTE: 'inTaintTime' is no longer used. This entry exists so all the calls don't have to be changed. + public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback) { - TaintedObject(false /*inTaintTime*/, pOriginator, pIdent, pCallback); + TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback); } public void TaintedObject(uint pOriginator, String pIdent, TaintCallback pCallback) { - TaintedObject(false /*inTaintTime*/, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback); - } - public void TaintedObject(bool inTaintTime, String pIdent, TaintCallback pCallback) - { - TaintedObject(inTaintTime, BSScene.DetailLogZero, pIdent, pCallback); - } - public void TaintedObject(bool inTaintTime, uint pOriginator, String pIdent, TaintCallback pCallback) - { - TaintedObject(inTaintTime, m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback); + TaintedObject(m_physicsLoggingEnabled ? pOriginator.ToString() : BSScene.DetailLogZero, pIdent, pCallback); } // Sometimes a potentially tainted operation can be used in and out of taint time. // This routine executes the command immediately if in taint-time otherwise it is queued. - public void TaintedObject(bool inTaintTime, string pOriginator, string pIdent, TaintCallback pCallback) + public void TaintedObject(string pOriginator, string pIdent, TaintCallback pCallback) { if (!m_initialized) return; - if (inTaintTime) + if (Monitor.TryEnter(PhysicsEngineLock)) + { + // If we can get exclusive access to the physics engine, just do the operation pCallback(); + Monitor.Exit(PhysicsEngineLock); + } else { + // The physics engine is busy, queue the operation lock (_taintLock) { _taintOperations.Add(new TaintCallbackEntry(pOriginator, pIdent, pCallback)); @@ -1133,14 +1273,21 @@ namespace OpenSim.Region.PhysicsModule.BulletS // When someone tries to change a property on a BSPrim or BSCharacter, the object queues // a callback into itself to do the actual property change. That callback is called // here just before the physics engine is called to step the simulation. - public void ProcessTaints() + // Returns the number of taints processed + // NOTE: Called while PhysicsEngineLock is locked + public int ProcessTaints() { - ProcessRegularTaints(); - ProcessPostTaintTaints(); + int ret = 0; + ret += ProcessRegularTaints(); + ret += ProcessPostTaintTaints(); + return ret; } - private void ProcessRegularTaints() + // Returns the number of taints processed + // NOTE: Called while PhysicsEngineLock is locked + private int ProcessRegularTaints() { + int ret = 0; if (m_initialized && _taintOperations.Count > 0) // save allocating new list if there is nothing to process { // swizzle a new list into the list location so we can process what's there @@ -1157,6 +1304,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS { DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", tcbe.originator, tcbe.ident); // DEBUG DEBUG DEBUG tcbe.callback(); + ret++; } catch (Exception e) { @@ -1165,6 +1313,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS } oldList.Clear(); } + return ret; } // Schedule an update to happen after all the regular taints are processed. @@ -1183,8 +1332,11 @@ namespace OpenSim.Region.PhysicsModule.BulletS } // Taints that happen after the normal taint processing but before the simulation step. - private void ProcessPostTaintTaints() + // Returns the number of taints processed + // NOTE: Called while PhysicsEngineLock is locked + private int ProcessPostTaintTaints() { + int ret = 0; if (m_initialized && _postTaintOperations.Count > 0) { Dictionary oldList; @@ -1200,6 +1352,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS { DetailLog("{0},BSScene.ProcessPostTaintTaints,doTaint,id={1}", DetailLogZero, kvp.Key); // DEBUG DEBUG DEBUG kvp.Value.callback(); + ret++; } catch (Exception e) { @@ -1208,20 +1361,8 @@ namespace OpenSim.Region.PhysicsModule.BulletS } oldList.Clear(); } + return ret; } - - // Only used for debugging. Does not change state of anything so locking is not necessary. - public bool AssertInTaintTime(string whereFrom) - { - if (!InTaintTime) - { - DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom); - m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom); - // Util.PrintCallStack(DetailLog); - } - return InTaintTime; - } - #endregion // Taints #region IPhysicsParameters diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs old mode 100644 new mode 100755 index b100273..86bf23f --- a/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapeCollection.cs @@ -30,7 +30,6 @@ using System.Text; using OMV = OpenMetaverse; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet; namespace OpenSim.Region.PhysicsModule.BulletS { @@ -76,8 +75,6 @@ public sealed class BSShapeCollection : IDisposable // Called at taint-time. public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim, PhysicalDestructionCallback bodyCallback) { - m_physicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape"); - bool ret = false; // This lock could probably be pushed down lower but building shouldn't take long @@ -230,6 +227,8 @@ public sealed class BSShapeCollection : IDisposable ret = CreateGeomMeshOrHull(prim, shapeCallback); } + m_physicsScene.PE.ResetBroadphasePool(m_physicsScene.World); // DEBUG DEBUG + return ret; } @@ -345,8 +344,6 @@ public sealed class BSShapeCollection : IDisposable if (!body.HasPhysicalBody) return; - m_physicsScene.AssertInTaintTime("BSShapeCollection.DereferenceBody"); - lock (m_collectionActivityLock) { if (DDetail) DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody,body={1}", body.ID, body); diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs b/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs old mode 100644 new mode 100755 index 086a412..e791b27 --- a/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSShapes.cs @@ -31,8 +31,8 @@ using System.Text; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModules.Meshing; -using OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet; +using OpenSim.Region.PhysicsModule.Meshing; +using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet; using OMV = OpenMetaverse; @@ -555,7 +555,9 @@ public class BSShapeMesh : BSShape { meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, false, // say it is not physical so a bounding box is not built - false // do not cache the mesh and do not use previously built versions + false, // do not cache the mesh and do not use previously built versions + false, + false ); } @@ -712,7 +714,7 @@ public class BSShapeHull : BSShape lock (physicsScene.mesher) { // Pass true for physicalness as this prevents the creation of bounding box which is not needed - meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, true /* isPhysical */, false /* shouldCache */); + meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, true /* isPhysical */, false /* shouldCache */, false, false); // If we should use the asset's hull info, fetch it out of the locked mesher if (meshData != null && BSParam.ShouldUseAssetHulls) diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs old mode 100644 new mode 100755 index 42fc11b..f72ad28 --- a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs @@ -141,14 +141,30 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys } // The passed position is relative to the base of the region. + // There are many assumptions herein that the heightmap increment is 1. public override float GetTerrainHeightAtXYZ(Vector3 pos) { float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET; - int mapIndex = (int)pos.Y * (int)m_mapInfo.sizeY + (int)pos.X; - try - { - ret = m_mapInfo.heightMap[mapIndex]; + try { + int baseX = (int)pos.X; + int baseY = (int)pos.Y; + int maxX = (int)m_mapInfo.sizeX; + int maxY = (int)m_mapInfo.sizeY; + float diffX = pos.X - (float)baseX; + float diffY = pos.Y - (float)baseY; + + float mapHeight1 = m_mapInfo.heightMap[baseY * maxY + baseX]; + float mapHeight2 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY + baseX]; + float mapHeight3 = m_mapInfo.heightMap[baseY * maxY + Math.Min(baseX + 1, maxX - 1)]; + float mapHeight4 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY + Math.Min(baseX + 1, maxX - 1)]; + + float Xrise = (mapHeight4 - mapHeight3) * diffX; + float Yrise = (mapHeight2 - mapHeight1) * diffY; + + ret = mapHeight1 + ((Xrise + Yrise) / 2f); + // m_physicsScene.DetailLog("{0},BSTerrainHeightMap,GetTerrainHeightAtXYZ,pos={1},{2}/{3}/{4}/{5},ret={6}", + // BSScene.DetailLogZero, pos, mapHeight1, mapHeight2, mapHeight3, mapHeight4, ret); } catch { diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainManager.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainMesh.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs old mode 100644 new mode 100755 index 3329395..308769b --- a/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/BulletSimData.cs @@ -100,6 +100,7 @@ public class BulletBody } } +// Handle to btCollisionObject - a shape that can be added to a btRidgidBody public class BulletShape { public BulletShape() diff --git a/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt b/OpenSim/Region/PhysicsModules/BulletS/BulletSimTODO.txt old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs b/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs old mode 100644 new mode 100755 index 2ba3c5a..ab5d4849 --- a/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/ExtendedPhysics.cs @@ -136,7 +136,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS return; } - + // Register as LSL functions all the [ScriptInvocation] marked methods. Comms.RegisterScriptInvocations(this); Comms.RegisterConstants(this); diff --git a/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs index 698be39..91ce468 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/Properties/AssemblyInfo.cs @@ -3,7 +3,7 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using Mono.Addins; -// General Information about an assembly is controlled through the following +// General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. [assembly: AssemblyTitle("OpenSim.Region.Physics.BulletSPlugin")] @@ -15,8 +15,8 @@ using Mono.Addins; [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] -// Setting ComVisible to false makes the types in this assembly not visible -// to COM components. If you need to access a type in this assembly from +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. [assembly: ComVisible(false)] @@ -26,11 +26,11 @@ using Mono.Addins; // Version information for an assembly consists of the following four values: // // Major Version -// Minor Version +// Minor Version // Build Number // Revision // -[assembly: AssemblyVersion("0.8.3.*")] +[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] [assembly: Addin("OpenSim.Region.PhysicsModule.BulletS", OpenSim.VersionInfo.VersionNumber)] [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BasicVehicles.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs old mode 100644 new mode 100755 index 0be1f4c..14d0bdc --- a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTests.cs @@ -1,56 +1,56 @@ -/* - * 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.Linq; -using System.Text; - -using NUnit.Framework; -using log4net; - -using OpenSim.Tests.Common; - -namespace OpenSim.Region.PhysicsModule.BulletS.Tests -{ -[TestFixture] -public class BulletSimTests : OpenSimTestCase -{ - // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1 - // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1 - - [TestFixtureSetUp] - public void Init() - { - } - - [TestFixtureTearDown] - public void TearDown() - { - } -} -} +/* + * 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.Linq; +using System.Text; + +using NUnit.Framework; +using log4net; + +using OpenSim.Tests.Common; + +namespace OpenSim.Region.PhysicsModule.BulletS.Tests +{ +[TestFixture] +public class BulletSimTests : OpenSimTestCase +{ + // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1 + // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1 + + [TestFixtureSetUp] + public void Init() + { + } + + [TestFixtureTearDown] + public void TearDown() + { + } +} +} diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs old mode 100644 new mode 100755 index 4eeea4d..3e8c1cd --- a/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/BulletSimTestsUtil.cs @@ -34,7 +34,7 @@ using Nini.Config; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModules.Meshing; +using OpenSim.Region.PhysicsModule.Meshing; using OpenSim.Region.Framework.Interfaces; using OpenMetaverse; @@ -80,13 +80,13 @@ public static class BulletSimTestsUtil } Vector3 regionExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, Constants.RegionHeight); - + RegionInfo info = new RegionInfo(); info.RegionName = "BSTestRegion"; info.RegionSizeX = info.RegionSizeY = info.RegionSizeZ = Constants.RegionSize; OpenSim.Region.Framework.Scenes.Scene scene = new OpenSim.Region.Framework.Scenes.Scene(info); - IMesher mesher = new OpenSim.Region.PhysicsModules.Meshing.Meshmerizer(); + IMesher mesher = new OpenSim.Region.PhysicsModule.Meshing.Meshmerizer(); INonSharedRegionModule mod = mesher as INonSharedRegionModule; mod.Initialise(openSimINI); mod.AddRegion(scene); diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs index c0cf19a..c2be568 100644 --- a/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs +++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/HullCreation.cs @@ -146,7 +146,7 @@ public class HullCreation : OpenSimTestCase uint torusLocalID = 125; PhysicsScene.AddPrimShape("testTorus", pbs, pos, size, rot, isPhys, torusLocalID); BSPrim primTypeTorus = (BSPrim)PhysicsScene.PhysObjects[torusLocalID]; - + // The actual prim shape creation happens at taint time PhysicsScene.ProcessTaints(); diff --git a/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs new file mode 100755 index 0000000..bfa95c1 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/BulletS/Tests/Raycast.cs @@ -0,0 +1,124 @@ +/* + * 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.Linq; +using System.Text; + +using NUnit.Framework; +using log4net; + +using OpenSim.Framework; +using OpenSim.Region.PhysicsModule.BulletS; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenSim.Tests.Common; + +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.BulletS.Tests +{ + [TestFixture] + public class BulletSimRaycast : OpenSimTestCase + { + // Documentation on attributes: http://www.nunit.org/index.php?p=attributes&r=2.6.1 + // Documentation on assertions: http://www.nunit.org/index.php?p=assertions&r=2.6.1 + + BSScene _physicsScene { get; set; } + BSPrim _targetSphere { get; set; } + Vector3 _targetSpherePosition { get; set; } + float _simulationTimeStep = 0.089f; + + uint _targetLocalID = 123; + + [TestFixtureSetUp] + public void Init() + { + Dictionary engineParams = new Dictionary(); + engineParams.Add("UseBulletRaycast", "true"); + _physicsScene = BulletSimTestsUtil.CreateBasicPhysicsEngine(engineParams); + + PrimitiveBaseShape pbs = PrimitiveBaseShape.CreateSphere(); + Vector3 pos = new Vector3(100.0f, 100.0f, 50f); + _targetSpherePosition = pos; + Vector3 size = new Vector3(10f, 10f, 10f); + pbs.Scale = size; + Quaternion rot = Quaternion.Identity; + bool isPhys = false; + + _physicsScene.AddPrimShape("TargetSphere", pbs, pos, size, rot, isPhys, _targetLocalID); + _targetSphere = (BSPrim)_physicsScene.PhysObjects[_targetLocalID]; + // The actual prim shape creation happens at taint time + _physicsScene.ProcessTaints(); + + } + + [TestFixtureTearDown] + public void TearDown() + { + if (_physicsScene != null) + { + // The Dispose() will also free any physical objects in the scene + _physicsScene.Dispose(); + _physicsScene = null; + } + } + + // There is a 10x10x10 sphere at <100,100,50> + // Shoot rays around the sphere and verify it hits and doesn't hit + // TestCase parameters are of start and of end and expected result + [TestCase(100f, 50f, 50f, 100f, 150f, 50f, true, "Pass through sphere from front")] + [TestCase(50f, 100f, 50f, 150f, 100f, 50f, true, "Pass through sphere from side")] + [TestCase(50f, 50f, 50f, 150f, 150f, 50f, true, "Pass through sphere diaginally")] + [TestCase(100f, 100f, 100f, 100f, 100f, 20f, true, "Pass through sphere from above")] + [TestCase(20f, 20f, 50f, 80f, 80f, 50f, false, "Not reach sphere")] + [TestCase(50f, 50f, 65f, 150f, 150f, 65f, false, "Passed over sphere")] + public void RaycastAroundObject(float fromX, float fromY, float fromZ, float toX, float toY, float toZ, bool expected, string msg) { + Vector3 fromPos = new Vector3(fromX, fromY, fromZ); + Vector3 toPos = new Vector3(toX, toY, toZ); + Vector3 direction = toPos - fromPos; + float len = Vector3.Distance(fromPos, toPos); + + List results = _physicsScene.RaycastWorld(fromPos, direction, len, 1); + + if (expected) { + // The test coordinates should generate a hit + Assert.True(results.Count != 0, msg + ": Did not return a hit but expected to."); + Assert.True(results.Count == 1, msg + ": Raycast returned not just one hit result."); + Assert.True(results[0].ConsumerID == _targetLocalID, msg + ": Raycast returned a collision object other than the target"); + } + else + { + // The test coordinates should not generate a hit + if (results.Count > 0) + { + Assert.False(results.Count > 0, msg + ": Returned a hit at " + results[0].Pos.ToString()); + } + } + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs index 7ad689e..bd1f75e 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/CTri.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -28,7 +28,7 @@ using System; using System.Collections.Generic; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class Wpoint { @@ -246,13 +246,13 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } private static float DistToPt(float3 p, float4 plane) - { - float x = p.x; - float y = p.y; - float z = p.z; - float d = x*plane.x + y*plane.y + z*plane.z + plane.w; - return d; - } + { + float x = p.x; + float y = p.y; + float z = p.z; + float d = x*plane.x + y*plane.y + z*plane.z + plane.w; + return d; + } private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane) { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs index 4140d25..0d22d71 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Concavity.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Text; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public static class Concavity { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs index 70c3a2b..810e95d 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexBuilder.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class DecompDesc { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs index 5046bce..a382f9d 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexDecomposition.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public delegate void ConvexDecompositionCallback(ConvexResult result); diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs index 44e3e50..300fed3 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/ConvexResult.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -28,7 +28,7 @@ using System; using System.Collections.Generic; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class ConvexResult { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs index 8a0164e..73fb252 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullClasses.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -28,7 +28,7 @@ using System; using System.Collections.Generic; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class HullResult { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs index d3f0052..6e73fd5 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullTriangle.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class HullTriangle : int3 { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs index 3903254..cf7161a 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/HullUtils.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public static class HullUtils { @@ -763,7 +763,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0)) { - // both endpoints coplanar + // both endpoints coplanar // must check a 3rd point to see if UNDER int e2 = e1 + 1; if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane) @@ -792,7 +792,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2)) { - // first is under 2nd is over + // first is under 2nd is over edgeflag[e0].undermap = (short)under_edge_count; tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; @@ -815,7 +815,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet vout = vertcountunder++; } under_edge_count++; - /// hmmm something to think about: i might be able to output this edge regarless of + /// hmmm something to think about: i might be able to output this edge regarless of // wheter or not we know v-in yet. ok i;ll try this now: tmpunderedges[under_edge_count].v = (byte)vout; tmpunderedges[under_edge_count].p = (byte)underplanescount; @@ -834,7 +834,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2)) { - // first is coplanar 2nd is over + // first is coplanar 2nd is over edgeflag[e0].undermap = -1; vout = vertflag[edge0.v].undermap; @@ -858,7 +858,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1)) { - // first is over next is under + // first is over next is under // new vertex!!! Debug.Assert(vin == -1); if (e0 < edge0.ea) @@ -901,7 +901,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0)) { - // first is over next is coplanar + // first is over next is coplanar edgeflag[e0].undermap = -1; vin = vertflag[edge1.v].undermap; @@ -1074,14 +1074,27 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet public static int maxdirfiltered(List p, int count, float3 dir, byte[] allow) { //Debug.Assert(count != 0); - int m = 0; - float currDotm = float3.dot(p[0], dir); + int m = -1; + float currDotm = 0; float currDoti; - while (allow[m] == 0) - m++; + for (int i = 0; i < count; i++) + { + if (allow[i] != 0) + { + currDotm = float3.dot(p[i], dir); + m = i; + break; + } + } - for (int i = 1; i < count; i++) + if(m == -1) + { + Debug.Assert(false); + return m; + } + + for (int i = m + 1; i < count; i++) { if (allow[i] != 0) { @@ -1093,7 +1106,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } } } - //Debug.Assert(m != -1); + +// Debug.Assert(m != -1); return m; } @@ -1112,8 +1126,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet { int mb; { - float s = (float)Math.Sin((3.14159264f / 180.0f) * (x)); - float c = (float)Math.Cos((3.14159264f / 180.0f) * (x)); + float s = (float)Math.Sin(0.01745329f * x); + float c = (float)Math.Cos(0.01745329f * x); mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); } if (ma == m && mb == m) @@ -1126,8 +1140,8 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet int mc = ma; for (float xx = x - 40.0f; xx <= x; xx += 5.0f) { - float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx)); - float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx)); + float s = (float)Math.Sin(0.01745329f * xx); + float c = (float)Math.Cos(0.01745329f * xx); int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); if (mc == m && md == m) { @@ -1176,7 +1190,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) { - Swap(ref p2, ref p3); + return new int4(p0, p1, p3, p2); } return new int4(p0, p1, p2, p3); } @@ -1207,12 +1221,12 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet int j; float3 bmin = new float3(verts[0]); float3 bmax = new float3(verts[0]); - List isextreme = new List(verts.Count); + byte[] isextreme = new byte[verts.Count]; byte[] allow = new byte[verts.Count]; for (j = 0; j < verts.Count; j++) { allow[j] = 1; - isextreme.Add(0); + isextreme[j] = 0; bmin = float3.VectorMin(bmin, verts[j]); bmax = float3.VectorMax(bmax, verts[j]); } @@ -1526,6 +1540,19 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } } + public static bool ComputeHull(List vertices, out List indices) + { + List tris = new List(); + + bool ret = calchull(vertices, out indices, 0, tris); + if (ret == false) + { + indices = new List(); + return false; + } + return true; + } + private static bool CleanupVertices(List svertices, out List vertices, float normalepsilon, out float3 scale) { const float EPSILON = 0.000001f; @@ -1609,7 +1636,7 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet addPoint(ref vcount, vertices, x2, y2, z2); addPoint(ref vcount, vertices, x1, y2, z2); - return true; // return cube + return true; // return cube } else { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs index da9ae0c..0f10e5f 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Plane.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class Plane { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs index 42f7a22..5915c29 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/PlaneTri.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public enum PlaneTriResult : int { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs index b9cd6f5..9b418d6 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs @@ -2,7 +2,7 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; -// General Information about an assembly is controlled through the following +// General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. [assembly: AssemblyTitle("ConvexDecompositionDotNet")] @@ -14,8 +14,8 @@ using System.Runtime.InteropServices; [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] -// Setting ComVisible to false makes the types in this assembly not visible -// to COM components. If you need to access a type in this assembly from +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. [assembly: ComVisible(false)] @@ -25,12 +25,12 @@ using System.Runtime.InteropServices; // Version information for an assembly consists of the following four values: // // Major Version -// Minor Version +// Minor Version // Build Number // Revision // -// You can specify all the values or you can default the Build and Revision Numbers +// You can specify all the values or you can default the Build and Revision Numbers // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] -[assembly: AssemblyVersion("0.8.3.*")] +[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs index 045f620..571677d 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/Quaternion.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class Quaternion : float4 { @@ -126,9 +126,9 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } public static Quaternion operator *(Quaternion a, float b) - { - return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b); - } + { + return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b); + } public static Quaternion normalize(Quaternion a) { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs index 9f56bc5..d8c10c7 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/SplitPlane.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -28,7 +28,7 @@ using System; using System.Collections.Generic; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class Rect3d { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs index bfe11e5..915bec6 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/VertexLookup.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -28,7 +28,7 @@ using System; using System.Collections.Generic; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class VertexPool { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs index e7358c1..c2c8940 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float2.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class float2 { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs index fde9b32..1ea003b 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class float3 : IEquatable { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs index c420fde..4b6cd5d 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float3x3.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -29,7 +29,7 @@ using System; using System.Collections.Generic; using System.Diagnostics; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class float3x3 { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs index b2b6fd3..1f2ab02 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class float4 { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs index 087eba7..85a8cf1 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/float4x4.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -30,7 +30,7 @@ using System.Collections.Generic; using System.Linq; using System.Text; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class float4x4 { @@ -127,88 +127,88 @@ namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet } public static float4x4 Inverse(float4x4 m) - { - float4x4 d = new float4x4(); - //float dst = d.x.x; - float[] tmp = new float[12]; // temp array for pairs - float[] src = new float[16]; // array of transpose source matrix - float det; // determinant - // transpose matrix - for (int i = 0; i < 4; i++) - { - src[i] = m[i].x; - src[i + 4] = m[i].y; - src[i + 8] = m[i].z; - src[i + 12] = m[i].w; - } - // calculate pairs for first 8 elements (cofactors) - tmp[0] = src[10] * src[15]; - tmp[1] = src[11] * src[14]; - tmp[2] = src[9] * src[15]; - tmp[3] = src[11] * src[13]; - tmp[4] = src[9] * src[14]; - tmp[5] = src[10] * src[13]; - tmp[6] = src[8] * src[15]; - tmp[7] = src[11] * src[12]; - tmp[8] = src[8] * src[14]; - tmp[9] = src[10] * src[12]; - tmp[10] = src[8] * src[13]; - tmp[11] = src[9] * src[12]; - // calculate first 8 elements (cofactors) - d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; - d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; - d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; - d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; - d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; - d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; - d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; - d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; - d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; - d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; - d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; - d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; - d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; - d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; - d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; - d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; - // calculate pairs for second 8 elements (cofactors) - tmp[0] = src[2]*src[7]; - tmp[1] = src[3]*src[6]; - tmp[2] = src[1]*src[7]; - tmp[3] = src[3]*src[5]; - tmp[4] = src[1]*src[6]; - tmp[5] = src[2]*src[5]; - tmp[6] = src[0]*src[7]; - tmp[7] = src[3]*src[4]; - tmp[8] = src[0]*src[6]; - tmp[9] = src[2]*src[4]; - tmp[10] = src[0]*src[5]; - tmp[11] = src[1]*src[4]; - // calculate second 8 elements (cofactors) - d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; - d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; - d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; - d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; - d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; - d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; - d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; - d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; - d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; - d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; - d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; - d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; - d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; - d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; - d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; - d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; - // calculate determinant + { + float4x4 d = new float4x4(); + //float dst = d.x.x; + float[] tmp = new float[12]; // temp array for pairs + float[] src = new float[16]; // array of transpose source matrix + float det; // determinant + // transpose matrix + for (int i = 0; i < 4; i++) + { + src[i] = m[i].x; + src[i + 4] = m[i].y; + src[i + 8] = m[i].z; + src[i + 12] = m[i].w; + } + // calculate pairs for first 8 elements (cofactors) + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + // calculate first 8 elements (cofactors) + d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; + d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; + d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; + d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; + d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; + d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; + d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; + d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; + d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; + d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; + d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; + d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; + d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; + d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; + d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; + d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; + // calculate pairs for second 8 elements (cofactors) + tmp[0] = src[2]*src[7]; + tmp[1] = src[3]*src[6]; + tmp[2] = src[1]*src[7]; + tmp[3] = src[3]*src[5]; + tmp[4] = src[1]*src[6]; + tmp[5] = src[2]*src[5]; + tmp[6] = src[0]*src[7]; + tmp[7] = src[3]*src[4]; + tmp[8] = src[0]*src[6]; + tmp[9] = src[2]*src[4]; + tmp[10] = src[0]*src[5]; + tmp[11] = src[1]*src[4]; + // calculate second 8 elements (cofactors) + d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; + d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; + d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; + d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; + d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; + d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; + d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; + d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; + d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; + d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; + d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; + d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; + d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; + d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; + d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; + d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; + // calculate determinant det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w; - // calculate matrix inverse - det = 1/det; + // calculate matrix inverse + det = 1/det; for (int j = 0; j < 4; j++) d[j] *= det; - return d; - } + return d; + } public static float4x4 MatrixRigidInverse(float4x4 m) { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs index 90624eb..e7bb6e7 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int3.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class int3 { diff --git a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs index e9320c0..9950be2 100644 --- a/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs +++ b/OpenSim/Region/PhysicsModules/ConvexDecompositionDotNet/int4.cs @@ -1,21 +1,21 @@ /* The MIT License - * + * * Copyright (c) 2010 Intel Corporation. * All rights reserved. * - * Based on the convexdecomposition library from + * Based on the convexdecomposition library from * by John W. Ratcliff and Stan Melax. - * + * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: - * + * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. - * + * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -27,7 +27,7 @@ using System; -namespace OpenSim.Region.PhysicsModule.ConvexDecompositionDotNet +namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet { public class int4 { diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs index 34a925d..a0e3b7f 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/HelperTypes.cs @@ -31,7 +31,7 @@ using System.Diagnostics; using System.Globalization; using OpenMetaverse; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModules.Meshing; +using OpenSim.Region.PhysicsModule.Meshing; public class Vertex : IComparable { diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs index bf397ee..42ba37e 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Mesh.cs @@ -33,7 +33,7 @@ using OpenSim.Region.PhysicsModules.SharedBase; using PrimMesher; using OpenMetaverse; -namespace OpenSim.Region.PhysicsModules.Meshing +namespace OpenSim.Region.PhysicsModule.Meshing { public class Mesh : IMesh { @@ -46,11 +46,36 @@ namespace OpenSim.Region.PhysicsModules.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.PhysicsModules.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,63 @@ namespace OpenSim.Region.PhysicsModules.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; + } + + // not functional + public Vector3 GetOBB() + { + return new Vector3(0.5f, 0.5f, 0.5f); + } + public void CalcNormals() { int iTriangles = m_triangles.Count; @@ -185,6 +259,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing public void getVertexListAsPtrToFloatArray(out IntPtr vertices, out int vertexStride, out int vertexCount) { // A vertex is 3 floats + vertexStride = 3 * sizeof(float); // If there isn't an unmanaged array allocated yet, do it now @@ -224,7 +299,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing { if (m_pinnedIndex.IsAllocated) return (int[])(m_pinnedIndex.Target); - + int[] result = getIndexListAsInt(); m_pinnedIndex = GCHandle.Alloc(result, GCHandleType.Pinned); // Inform the garbage collector of this unmanaged allocation so it can schedule @@ -282,7 +357,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing { if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to Append to a pinned Mesh"); - + if (!(newMesh is Mesh)) return; @@ -295,7 +370,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing { if (m_pinnedIndex.IsAllocated || m_pinnedVertexes.IsAllocated || m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); - + foreach (Vertex v in m_vertices.Keys) { if (v == null) diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs index 3a944d1..0d4b6b9 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/Meshmerizer.cs @@ -44,7 +44,7 @@ using log4net; using Nini.Config; using Mono.Addins; -namespace OpenSim.Region.PhysicsModules.Meshing +namespace OpenSim.Region.PhysicsModule.Meshing { [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "Meshmerizer")] public class Meshmerizer : IMesher, INonSharedRegionModule @@ -66,7 +66,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing private bool cacheSculptMaps = true; private string decodedSculptMapPath = null; - private bool useMeshiesPhysicsMesh = false; + private bool useMeshiesPhysicsMesh = true; private float minSizeForComplexMesh = 0.2f; // prims with all dimensions smaller than this will have a bounding box mesh @@ -99,7 +99,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing IConfig mesh_config = source.Configs["Mesh"]; - decodedSculptMapPath = "../caches/" + config.GetString("DecodedSculptMapPath", "j2kDecodeCache"); + decodedSculptMapPath = config.GetString("DecodedSculptMapPath", "j2kDecodeCache"); cacheSculptMaps = config.GetBoolean("CacheSculptMaps", cacheSculptMaps); if (mesh_config != null) { @@ -247,13 +247,13 @@ namespace OpenSim.Region.PhysicsModules.Meshing private void AddSubMesh(OSDMap subMeshData, Vector3 size, List coords, List faces) { // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap)); - + // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no // geometry for this submesh. if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"])) return; - + OpenMetaverse.Vector3 posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); OpenMetaverse.Vector3 posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); ushort faceIndexOffset = (ushort)coords.Count; @@ -264,15 +264,15 @@ namespace OpenSim.Region.PhysicsModules.Meshing ushort uX = Utils.BytesToUInt16(posBytes, i); ushort uY = Utils.BytesToUInt16(posBytes, i + 2); ushort uZ = Utils.BytesToUInt16(posBytes, i + 4); - + Coord c = new Coord( Utils.UInt16ToFloat(uX, posMin.X, posMax.X) * size.X, Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y) * size.Y, Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z) * size.Z); - + coords.Add(c); } - + byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); for (int i = 0; i < triangleBytes.Length; i += 6) { @@ -436,9 +436,9 @@ namespace OpenSim.Region.PhysicsModules.Meshing int convexSize = convexBlock["size"].AsInteger(); byte[] convexBytes = new byte[convexSize]; - + System.Buffer.BlockCopy(primShape.SculptData, convexOffset, convexBytes, 0, convexSize); - + try { convexBlockOsd = DecompressOsd(convexBytes); @@ -449,7 +449,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing //return false; } } - + if (convexBlockOsd != null && convexBlockOsd is OSDMap) { convexBlock = convexBlockOsd as OSDMap; @@ -762,7 +762,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing { PrimMesh primMesh; coords = new List(); - faces = new List(); + faces = new List(); float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f; float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f; @@ -947,11 +947,21 @@ namespace OpenSim.Region.PhysicsModules.Meshing return CreateMesh(primName, primShape, size, lod, false, true); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, false); + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { return CreateMesh(primName, primShape, size, lod, isPhysical, true); } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, isPhysical, true); + } + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache) { #if SPAM @@ -984,7 +994,7 @@ namespace OpenSim.Region.PhysicsModules.Meshing if ((!isPhysical) && size.X < minSizeForComplexMesh && size.Y < minSizeForComplexMesh && size.Z < minSizeForComplexMesh) { #if SPAM - m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " + + m_log.Debug("Meshmerizer: prim " + primName + " has a size of " + size.ToString() + " which is below threshold of " + minSizeForComplexMesh.ToString() + " - creating simple bounding box"); #endif mesh = CreateBoundingBoxMesh(mesh); @@ -1005,6 +1015,13 @@ namespace OpenSim.Region.PhysicsModules.Meshing return mesh; } + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return null; + } + public void ReleaseMesh(IMesh imesh) { } + public void ExpireReleaseMeshs() { } + public void ExpireFileCache() { } } } diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs index 4049ee1..fd2b1ea 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/PrimMesher.cs @@ -445,7 +445,7 @@ namespace PrimMesher new Angle(1.0f, 1.0f, 0.0f) }; - private static Coord[] normals4 = + private static Coord[] normals4 = { new Coord(0.5f, 0.5f, 0.0f).Normalize(), new Coord(-0.5f, 0.5f, 0.0f).Normalize(), @@ -2066,7 +2066,7 @@ namespace PrimMesher /// DEPRICATED - use Extrude(PathType.Linear) instead /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism. /// - /// + /// public void ExtrudeLinear() { this.Extrude(PathType.Linear); @@ -2077,7 +2077,7 @@ namespace PrimMesher /// DEPRICATED - use Extrude(PathType.Circular) instead /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring. /// - /// + /// public void ExtrudeCircular() { this.Extrude(PathType.Circular); diff --git a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs index 740424e..01d11f4 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/SculptMap.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Meshmerizer/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); - grPhoto.DrawImage(srcImage, - new Rectangle(0, 0, destWidth, destHeight), - new Rectangle(0, 0, srcImage.Width, srcImage.Height), - GraphicsUnit.Pixel); + Color c; + float xscale = srcImage.Width / destWidth; + float yscale = srcImage.Height / destHeight; - grPhoto.Dispose(); + 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) + { + } + + sx += xscale; + } + sy += yscale; + } + srcImage.Dispose(); return scaledImage; } + + } + } -} #endif diff --git a/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs index b4bdb5a..5c257e6 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/Properties/AssemblyInfo.cs @@ -3,10 +3,10 @@ using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using Mono.Addins; -// General Information about an assembly is controlled through the following +// General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. -[assembly: AssemblyTitle("OpenSim.Region.PhysicsModules.Meshing")] +[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.Meshing")] [assembly: AssemblyDescription("")] [assembly: AssemblyConfiguration("")] [assembly: AssemblyCompany("http://opensimulator.org")] @@ -15,8 +15,8 @@ using Mono.Addins; [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] -// Setting ComVisible to false makes the types in this assembly not visible -// to COM components. If you need to access a type in this assembly from +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. [assembly: ComVisible(false)] @@ -26,11 +26,11 @@ using Mono.Addins; // Version information for an assembly consists of the following four values: // // Major Version -// Minor Version +// Minor Version // Build Number // Revision // -[assembly: AssemblyVersion("0.8.3.*")] +[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] -[assembly: Addin("OpenSim.Region.PhysicsModules.Meshing", OpenSim.VersionInfo.VersionNumber)] +[assembly: Addin("OpenSim.Region.PhysicsModule.Meshing", OpenSim.VersionInfo.VersionNumber)] [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs b/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs index 0a3b3a4..dbf4f7e 100644 --- a/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs +++ b/OpenSim/Region/PhysicsModules/Meshing/ZeroMesher.cs @@ -47,7 +47,7 @@ using log4net; * it's always availabe and thus the default in case of configuration errors */ -namespace OpenSim.Region.PhysicsModules.Meshing +namespace OpenSim.Region.PhysicsModule.Meshing { [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ZeroMesher")] @@ -110,23 +110,36 @@ namespace OpenSim.Region.PhysicsModules.Meshing #region IMesher public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) { - return CreateMesh(primName, primShape, size, lod, false, false); + return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex,bool forOde) { - return CreateMesh(primName, primShape, size, lod, false, false); + return CreateMesh(primName, primShape, size, lod, false); } - public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache) + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) { // Remove the reference to the encoded JPEG2000 data so it can be GCed primShape.SculptData = OpenMetaverse.Utils.EmptyBytes; return null; } - #endregion + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + return null; + } + public void ReleaseMesh(IMesh mesh) { } + public void ExpireReleaseMeshs() { } + public void ExpireFileCache() { } + + #endregion } } diff --git a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs index 7869739..3691d96 100644 --- a/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/Ode/AssemblyInfo.cs @@ -56,7 +56,7 @@ using Mono.Addins; // 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.8.2.*")] +[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] [assembly: Addin("OpenSim.Region.PhysicsModule.ODE", OpenSim.VersionInfo.VersionNumber)] [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs b/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs new file mode 100644 index 0000000..c851b12 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/Ode/ODEApi.cs @@ -0,0 +1,2025 @@ +/* + * 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/ + * changes by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; +using OMV = OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ODE +{ +//#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; + + public const uint CONTACTS_UNIMPORTANT = 0x80000000; + + #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, + ubtTerrainClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 5 + } + + 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 dReal OSTerrainGetHeight(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 = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiOSTerrain(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 uint 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 uint 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)); } + } + public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) + { + Vector3 vtmp = GeomGetPosition(geom); + return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) + { + Quaternion qtmp; + GeomCopyQuaternion(geom, out qtmp); + return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); + } + + [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 = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomOSTerrainDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomOSTerrainGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainSetHeightfieldData(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, uint bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, uint 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 IntPtr iGetConfiguration(); + + public static string GetConfiguration() + { + IntPtr ptr = iGetConfiguration(); + string s = Marshal.PtrToStringAnsi(ptr); + return s; + } + + [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/PhysicsModules/Ode/ODECharacter.cs b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs index b35c299..98bfd1c 100644 --- a/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs +++ b/OpenSim/Region/PhysicsModules/Ode/ODECharacter.cs @@ -29,7 +29,6 @@ using System; using System.Collections.Generic; using System.Reflection; using OpenMetaverse; -using Ode.NET; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; using log4net; @@ -150,7 +149,7 @@ namespace OpenSim.Region.PhysicsModule.ODE /// Collision geometry /// internal IntPtr Shell { get; private set; } - + private IntPtr Amotor = IntPtr.Zero; private d.Mass ShellMass; @@ -238,7 +237,7 @@ namespace OpenSim.Region.PhysicsModule.ODE m_tainted_isPhysical = true; // new tainted status: need to create ODE information _parent_scene.AddPhysicsActorTaint(this); - + Name = avName; } @@ -280,7 +279,7 @@ namespace OpenSim.Region.PhysicsModule.ODE public override bool IsPhysical { - get { return false; } + get { return m_isPhysical; } set { return; } } @@ -462,7 +461,7 @@ namespace OpenSim.Region.PhysicsModule.ODE value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - m_taintPosition = value; + m_taintPosition = value; _parent_scene.AddPhysicsActorTaint(this); } else @@ -595,7 +594,7 @@ namespace OpenSim.Region.PhysicsModule.ODE public override void delink() {} - public override void LockAngularMotion(Vector3 axis) {} + public override void LockAngularMotion(byte axislocks) {} // This code is very useful. Written by DanX0r. We're just not using it right now. // Commented out to prevent a warning. @@ -797,7 +796,7 @@ namespace OpenSim.Region.PhysicsModule.ODE internal void Move(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 @@ -812,7 +811,7 @@ namespace OpenSim.Region.PhysicsModule.ODE d.Vector3 localpos = d.BodyGetPosition(Body); Vector3 localPos = new Vector3(localpos.X, localpos.Y, localpos.Z); - + if (!localPos.IsFinite()) { m_log.WarnFormat( @@ -1105,8 +1104,8 @@ namespace OpenSim.Region.PhysicsModule.ODE // lock (OdeScene.UniversalColliderSyncObject) Shell = d.CreateCapsule(_parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); - d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); - d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + d.GeomSetCategoryBits(Shell, (uint)m_collisionCategories); + d.GeomSetCollideBits(Shell, (uint)m_collisionFlags); d.MassSetCapsuleTotal(out ShellMass, m_mass, 2, CAPSULE_RADIUS, CAPSULE_LENGTH); Body = d.BodyCreate(_parent_scene.world); @@ -1249,18 +1248,18 @@ namespace OpenSim.Region.PhysicsModule.ODE } public override Vector3 PIDTarget { set { return; } } - public override bool PIDActive - { + public override bool PIDActive + { get { return false; } - set { return; } + set { return; } } public override float PIDTau { set { return; } } public override float PIDHoverHeight { set { return; } } - public override bool PIDHoverActive { set { return; } } + public override bool PIDHoverActive {get {return false;} 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; } } @@ -1291,7 +1290,7 @@ namespace OpenSim.Region.PhysicsModule.ODE m_eventsubscription = 0; } - internal void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { if (m_eventsubscription > 0) { @@ -1370,7 +1369,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // m_log.DebugFormat( // "[ODE CHARACTER]: Changing capsule size from {0} to {1} for {2}", // CAPSULE_LENGTH, m_tainted_CAPSULE_LENGTH, Name); - + m_pidControllerActive = true; // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs index 8f8e2bd..7e95d7f 100644 --- a/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs +++ b/OpenSim/Region/PhysicsModules/Ode/ODEDynamics.cs @@ -44,10 +44,10 @@ using System.Reflection; using System.Runtime.InteropServices; using log4net; using OpenMetaverse; -using Ode.NET; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; + namespace OpenSim.Region.PhysicsModule.ODE { public class ODEDynamics @@ -607,6 +607,13 @@ namespace OpenSim.Region.PhysicsModule.ODE m_body = pBody; } + internal void Stop() + { + m_lastLinearVelocityVector = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_lastPositionVector = d.BodyGetPosition(Body); + } + internal void Step(float pTimestep, OdeScene pParentScene) { if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) @@ -907,7 +914,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // Sum velocities m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection - + if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) { m_lastAngularVelocity.X = 0; diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs b/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs new file mode 100644 index 0000000..22fc84d --- /dev/null +++ b/OpenSim/Region/PhysicsModules/Ode/ODEModule.cs @@ -0,0 +1,89 @@ +using System; +using System.Reflection; +using log4net; +using Nini.Config; +using Mono.Addins; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; + +namespace OpenSim.Region.PhysicsModule.ODE +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ODEPhysicsScene")] + public class OdeModule : INonSharedRegionModule + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_Enabled = false; + private IConfigSource m_config; + private OdeScene m_scene; + + #region INonSharedRegionModule + + public string Name + { + get { return "OpenDynamicsEngine"; } + } + + public string Version + { + get { return "1.0"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void Initialise(IConfigSource source) + { + IConfig config = source.Configs["Startup"]; + if (config != null) + { + string physics = config.GetString("physics", string.Empty); + if (physics == Name) + { + m_config = source; + m_Enabled = true; + } + } + } + + public void Close() + { + } + + public void AddRegion(Scene scene) + { + if (!m_Enabled) + return; + + if (Util.IsWindows()) + Util.LoadArchSpecificWindowsDll("ode.dll"); + + // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to + // http://opensimulator.org/mantis/view.php?id=2750). + d.InitODE(); + + m_scene = new OdeScene(scene, m_config, Name, Version); + } + + public void RemoveRegion(Scene scene) + { + if (!m_Enabled || m_scene == null) + return; + + m_scene.Dispose(); + m_scene = null; + } + + public void RegionLoaded(Scene scene) + { + if (!m_Enabled || m_scene == null) + return; + + m_scene.RegionLoaded(); + } + #endregion + } +} diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs index 0b9c45f..8934330 100644 --- a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs +++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs @@ -33,7 +33,7 @@ * 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 + * 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. @@ -48,7 +48,6 @@ using System.Runtime.InteropServices; using System.Threading; using log4net; using OpenMetaverse; -using Ode.NET; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; @@ -83,8 +82,15 @@ namespace OpenSim.Region.PhysicsModule.ODE set { m_isphysical = value; - if (!m_isphysical) // Zero the remembered last velocity + if (!m_isphysical) + { + _zeroFlag = true; // Zero the remembered last velocity m_lastVelocity = Vector3.Zero; + _acceleration = Vector3.Zero; + _velocity = Vector3.Zero; + m_taintVelocity = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } } } @@ -104,10 +110,12 @@ namespace OpenSim.Region.PhysicsModule.ODE private Vector3 m_taintVelocity; private Vector3 m_taintTorque; private Quaternion m_taintrot; - private Vector3 m_angularlock = Vector3.One; - private Vector3 m_taintAngularLock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + private byte m_taintAngularLock = 0; + private byte m_angularlock = 0; + private bool m_assetFailed = false; private Vector3 m_PIDTarget; @@ -125,7 +133,7 @@ namespace OpenSim.Region.PhysicsModule.ODE 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 float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -162,7 +170,7 @@ namespace OpenSim.Region.PhysicsModule.ODE private PrimitiveBaseShape _pbs; private OdeScene _parent_scene; - + /// /// The physics space which contains prim geometries /// @@ -195,7 +203,6 @@ namespace OpenSim.Region.PhysicsModule.ODE public int m_interpenetrationcount { get; private set; } internal float m_collisionscore; public int m_roundsUnderMotionThreshold { get; private set; } - private int m_crossingfailures; public bool outofBounds { get; private set; } private float m_density = 10.000006836f; // Aluminum g/cm3; @@ -350,12 +357,12 @@ namespace OpenSim.Region.PhysicsModule.ODE if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); + d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } _parent_scene.geom_name_map[prim_geom] = Name; @@ -423,7 +430,7 @@ namespace OpenSim.Region.PhysicsModule.ODE if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); + d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); } else { @@ -431,12 +438,12 @@ namespace OpenSim.Region.PhysicsModule.ODE m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); } - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - + // disconnect from world gravity so we can apply buoyancy d.BodySetGravityMode (Body, false); @@ -445,7 +452,7 @@ namespace OpenSim.Region.PhysicsModule.ODE m_disabled = false; // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + if (m_angularlock != 0 && _parent == null) { createAMotor(m_angularlock); } @@ -467,8 +474,8 @@ namespace OpenSim.Region.PhysicsModule.ODE 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: @@ -504,16 +511,16 @@ namespace OpenSim.Region.PhysicsModule.ODE else if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - //a tube + //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: @@ -572,7 +579,7 @@ namespace OpenSim.Region.PhysicsModule.ODE 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) { @@ -816,8 +823,8 @@ namespace OpenSim.Region.PhysicsModule.ODE } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } d.BodyDestroy(Body); @@ -838,7 +845,7 @@ namespace OpenSim.Region.PhysicsModule.ODE else { _parent_scene.DeactivatePrim(this); - + m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); @@ -850,8 +857,8 @@ namespace OpenSim.Region.PhysicsModule.ODE else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } Body = IntPtr.Zero; @@ -909,7 +916,7 @@ namespace OpenSim.Region.PhysicsModule.ODE else { _triMeshData = d.GeomTriMeshDataCreate(); - + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); d.GeomTriMeshDataPreprocess(_triMeshData); m_MeshToTriMeshMap[mesh] = _triMeshData; @@ -919,7 +926,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); } catch (AccessViolationException) { @@ -967,7 +974,7 @@ Console.WriteLine("ZProcessTaints for " + Name); rotate(); } } - + if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) changePhysicsStatus(); @@ -1001,7 +1008,7 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_taintCollidesWater != m_collidesWater) changefloatonwater(); - if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) + if (m_taintAngularLock != m_angularlock) changeAngularLock(); } @@ -1017,10 +1024,8 @@ Console.WriteLine("ZProcessTaints for " + Name); //If we have a parent then we're not authorative here if (_parent == null) { - if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) + if (m_taintAngularLock != 0) { - //d.BodySetFiniteRotationMode(Body, 0); - //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); createAMotor(m_taintAngularLock); } else @@ -1034,7 +1039,6 @@ Console.WriteLine("ZProcessTaints for " + Name); } } - // Store this for later in case we get turned into a separate body m_angularlock = m_taintAngularLock; } @@ -1070,7 +1074,7 @@ Console.WriteLine("ZProcessTaints for " + Name); else if (_parent != null && m_taintparent == null) { //Console.WriteLine(" changelink B"); - + if (_parent is OdePrim) { OdePrim obj = (OdePrim)_parent; @@ -1078,16 +1082,16 @@ Console.WriteLine("ZProcessTaints for " + Name); childPrim = false; //_parent = null; } - + /* if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) d.JointGroupDestroy(_linkJointGroup); - + _linkJointGroup = (IntPtr)0; m_linkJoint = (IntPtr)0; */ } - + _parent = m_taintparent; m_taintPhysics = IsPhysical; } @@ -1145,12 +1149,12 @@ Console.WriteLine("ZProcessTaints for " + Name); if (prm.m_assetFailed) { d.GeomSetCategoryBits(prm.prim_geom, 0); - d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits); } else { - d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); - d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); } d.Quaternion quat = new d.Quaternion(); @@ -1194,14 +1198,14 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); + d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); } else { //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); //Console.WriteLine(" Post GeomSetCategoryBits 2"); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } d.Quaternion quat2 = new d.Quaternion(); @@ -1230,7 +1234,8 @@ Console.WriteLine("ZProcessTaints for " + Name); m_disabled = false; // The body doesn't already have a finite rotation mode set here - if ((!m_angularlock.ApproxEquals(Vector3.One, 0f)) && _parent == null) + // or remove + if (_parent == null) { createAMotor(m_angularlock); } @@ -1340,7 +1345,7 @@ Console.WriteLine("ZProcessTaints for " + Name); // 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. @@ -1354,7 +1359,7 @@ Console.WriteLine("ZProcessTaints for " + Name); // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are // selected and disabled. then, due to a thread switch, the selection processing is // interrupted and the physics engine continues to simulate, so the last 50 items, whose - // selection was not yet processed, continues to simulate. this wakes up ALL of the + // selection was not yet processed, continues to simulate. this wakes up ALL of the // first 50 again. then the last 50 are disabled. then the first 50, which were just woken // up, start simulating again, which in turn wakes up the last 50. @@ -1370,8 +1375,8 @@ Console.WriteLine("ZProcessTaints for " + Name); } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } if (IsPhysical) @@ -1396,12 +1401,12 @@ Console.WriteLine("ZProcessTaints for " + Name); if (m_assetFailed) { d.GeomSetCategoryBits(prim_geom, 0); - d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); + d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); } else { - d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } if (IsPhysical) @@ -1560,7 +1565,7 @@ Console.WriteLine("CreateGeom:"); private void changeadd() { // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); - + int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); @@ -1686,6 +1691,9 @@ Console.WriteLine(" JointCreateFixed"); float fy = 0; float fz = 0; + if (outofBounds) + return; + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. { if (m_vehicle.Type != Vehicle.TYPE_NONE) @@ -1697,31 +1705,16 @@ Console.WriteLine(" JointCreateFixed"); { //Console.WriteLine("Move " + Name); if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here -// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) -// { -// d.Vector3 avel2 = d.BodyGetAngularVel(Body); -// /* -// if (m_angularlock.X == 1) -// avel2.X = 0; -// if (m_angularlock.Y == 1) -// avel2.Y = 0; -// if (m_angularlock.Z == 1) -// avel2.Z = 0; -// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); -// */ -// } - //float PID_P = 900.0f; float m_mass = CalculateMass(); // fz = 0f; //m_log.Info(m_collisionFlags.ToString()); - + //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up // gravityz multiplier = 1 - m_buoyancy fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; @@ -1736,7 +1729,7 @@ Console.WriteLine(" JointCreateFixed"); fz = 0f; // no lock; for now it's only called from within Simulate() - + // If the PID Controller isn't active then we set our force // calculating base velocity to the current position @@ -1745,7 +1738,7 @@ Console.WriteLine(" JointCreateFixed"); //PID_G = PID_G / m_PIDTau; m_PIDTau = 1; } - + if ((PID_G - m_PIDTau) <= 0) { PID_G = m_PIDTau + 1; @@ -1768,7 +1761,7 @@ Console.WriteLine(" JointCreateFixed"); 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 @@ -1789,7 +1782,7 @@ Console.WriteLine(" JointCreateFixed"); // We're flying and colliding with something fx = ((_target_velocity.X) - vel.X) * (PID_D); fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); @@ -1800,7 +1793,7 @@ Console.WriteLine(" JointCreateFixed"); if (m_useHoverPID && !PIDActive) { //Console.WriteLine("Hover " + Name); - + // If we're using the PID controller, then we have no gravity fz = (-1 * _parent_scene.gravityz) * m_mass; @@ -1857,7 +1850,7 @@ Console.WriteLine(" JointCreateFixed"); 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 @@ -1895,7 +1888,7 @@ Console.WriteLine(" JointCreateFixed"); { // 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. + // and the body should fall again. d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0, 0, 0); enableBodySoft(); @@ -1904,7 +1897,7 @@ Console.WriteLine(" JointCreateFixed"); // 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) @@ -1923,7 +1916,7 @@ Console.WriteLine(" JointCreateFixed"); // _zeroPosition = d.BodyGetPosition(Body); return; //Console.WriteLine("Nothing " + Name); - + } } @@ -1940,8 +1933,8 @@ Console.WriteLine(" JointCreateFixed"); d.BodySetQuaternion(Body, ref myrot); if (IsPhysical) { - if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); + // create or remove locks + createAMotor(m_angularlock); } } else @@ -1949,7 +1942,7 @@ Console.WriteLine(" JointCreateFixed"); // daughter prim, do Geom set d.GeomSetQuaternion(prim_geom, ref myrot); } - + resetCollisionAccounting(); m_taintrot = _orientation; } @@ -2087,7 +2080,7 @@ Console.WriteLine(" JointCreateFixed"); else m_assetFailed = false; } - + } CreateGeom(m_targetSpace, mesh); @@ -2140,10 +2133,10 @@ Console.WriteLine(" JointCreateFixed"); } if (m_assetFailed) - d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); + d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits); else - d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); } /// /// Change prim in response to a shape taint. @@ -2319,7 +2312,7 @@ Console.WriteLine(" JointCreateFixed"); } d.BodyEnable(Body); d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); - + } m_angularforcelist.Clear(); } @@ -2349,7 +2342,7 @@ Console.WriteLine(" JointCreateFixed"); d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); } } - + //resetCollisionAccounting(); } @@ -2659,16 +2652,46 @@ Console.WriteLine(" JointCreateFixed"); public override void CrossingFailure() { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + /* + m_crossingfailures++; + if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); + } + */ + + d.AllocateODEDataForThread(0U); + + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + + if (Body != IntPtr.Zero) { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); } + + if(m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); // this also updates vehicle last position from the body position + + enableBodySoft(); + + outofBounds = false; + base.RequestPhysicsterseUpdate(); + } public override float Buoyancy @@ -2687,32 +2710,23 @@ Console.WriteLine(" JointCreateFixed"); m_taintparent = null; } - public override void LockAngularMotion(Vector3 axis) + public override void LockAngularMotion(byte axislocks) { - // 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); - m_taintAngularLock = axis; - } - else - { - m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); - } + // m_log.DebugFormat("[axislocks]: {0}", axislocks); + m_taintAngularLock = axislocks; } internal void UpdatePositionAndVelocity() { // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (outofBounds) + return; if (_parent == null) { Vector3 pv = Vector3.Zero; bool lastZeroFlag = _zeroFlag; float m_minvelocity = 0; - if (Body != (IntPtr)0) // FIXME -> or if it is a joint + if (Body != IntPtr.Zero) // FIXME -> or if it is a joint { d.Vector3 vec = d.BodyGetPosition(Body); d.Quaternion ori = d.BodyGetQuaternion(Body); @@ -2723,12 +2737,6 @@ Console.WriteLine(" JointCreateFixed"); Vector3 l_position = Vector3.Zero; Quaternion l_orientation = Quaternion.Identity; - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } - m_lastposition = _position; m_lastorientation = _orientation; @@ -2740,26 +2748,6 @@ Console.WriteLine(" JointCreateFixed"); l_orientation.Z = ori.Z; l_orientation.W = ori.W; - if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) - { - //base.RaiseOutOfBounds(l_position); - - if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) - base.RequestPhysicsterseUpdate(); - return; - } - else - { - if (_parent == null) - base.RaiseOutOfBounds(l_position); - return; - } - } - if (l_position.Z < 0) { // This is so prim that get lost underground don't fall forever and suck up @@ -2769,8 +2757,6 @@ Console.WriteLine(" JointCreateFixed"); // It's a hack and will generate a console message if it fails. //IsPhysical = false; - if (_parent == null) - base.RaiseOutOfBounds(_position); _acceleration.X = 0; _acceleration.Y = 0; @@ -2784,16 +2770,65 @@ Console.WriteLine(" JointCreateFixed"); m_rotationalVelocity.Z = 0; if (_parent == null) + base.RaiseOutOfBounds(_position); + + if (_parent == null) base.RequestPhysicsterseUpdate(); m_throttleUpdates = false; throttleCounter = 0; _zeroFlag = true; //outofBounds = true; + return; + } + + if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) + { + //base.RaiseOutOfBounds(l_position); + /* + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; + } + else + { + if (_parent == null) + base.RaiseOutOfBounds(l_position); + return; + } + */ + outofBounds = true; + // part near the border on outside + if (l_position.X < 0) + Util.Clamp(l_position.X, -0.1f, -2f); + else + Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + if (l_position.Y < 0) + Util.Clamp(l_position.Y, -0.1f, -2f); + else + Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + + d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z); + + // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.BodySetLinearVel(Body, 0, 0, 0); + disableBodySoft(); + + _position = l_position; + // tell framework to fix it + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; } + //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + Name + " = " + Adiff); + //Console.WriteLine("Adiff " + Name + " = " + Adiff); if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) @@ -2864,12 +2899,12 @@ Console.WriteLine(" JointCreateFixed"); _acceleration = ((_velocity - m_lastVelocity) / 0.1f); _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); - - // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... - // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. + + // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... + // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles // adding these logical exclusion situations to maintain this where I think it was intended to be. - if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) + if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) { m_minvelocity = 0.5f; } @@ -2938,8 +2973,8 @@ Console.WriteLine(" JointCreateFixed"); { } - public override Vector3 PIDTarget - { + public override Vector3 PIDTarget + { set { if (value.IsFinite()) @@ -2948,16 +2983,17 @@ Console.WriteLine(" JointCreateFixed"); } else m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); - } + } } + public override bool PIDActive { get; set; } 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 bool PIDHoverActive { get { return m_useHoverPID;} 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; } } @@ -2966,7 +3002,7 @@ Console.WriteLine(" JointCreateFixed"); public override float APIDDamping{ set { return; } } - private void createAMotor(Vector3 axis) + private void createAMotor(byte axislock) { if (Body == IntPtr.Zero) return; @@ -2977,150 +3013,105 @@ Console.WriteLine(" JointCreateFixed"); Amotor = IntPtr.Zero; } - float axisnum = 3; - - axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); - - // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); - - - // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. - d.Mass objMass; - d.MassSetZero(out objMass); - DMassCopy(ref pMass, ref objMass); - - //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - Matrix4 dMassMat = FromDMass(objMass); - - Matrix4 mathmat = Inverse(dMassMat); - - /* - //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); - - mathmat = Inverse(mathmat); - - - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - mathmat = Inverse(mathmat); - */ - if (axis.X == 0) - { - mathmat.M33 = 50.0000001f; - //objMass.I.M22 = 0; - } - if (axis.Y == 0) - { - mathmat.M22 = 50.0000001f; - //objMass.I.M11 = 0; - } - if (axis.Z == 0) - { - mathmat.M11 = 50.0000001f; - //objMass.I.M00 = 0; - } - - + if(axislock == 0) + return; - mathmat = Inverse(mathmat); - objMass = FromMatrix4(mathmat, ref objMass); - //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); - - //return; - if (d.MassCheck(ref objMass)) - { - d.BodySetMass(Body, ref objMass); - } - else - { - //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); - } + int axisnum = 0; + bool axisX = false; + bool axisY = false; + bool axisZ = false; + if((axislock & 0x02) != 0) + { + axisnum++; + axisX = true; + } + if((axislock & 0x04) != 0) + { + axisnum++; + axisY = true; + } + if((axislock & 0x08) != 0) + { + axisnum++; + axisZ = true; + } - if (axisnum <= 0) + if(axisnum == 0) return; - // int dAMotorEuler = 1; + // 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,(int)axisnum); - int i = 0; + d.JointSetAMotorNumAxes(Amotor, axisnum); - if (axis.X == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); - i++; - } + // get current orientation to lock - if (axis.Y == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); - i++; - } + 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; - if (axis.Z == 0) - { - d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); + int i = 0; + int j = 0; + if (axisX) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axisY) + { + 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, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); i++; + j += 256; } - for (int j = 0; j < (int)axisnum; j++) + if (axisZ) { - //d.JointSetAMotorAngle(Amotor, j, 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, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); } - - //d.JointSetAMotorAngle(Amotor, 1, 0); - //d.JointSetAMotorAngle(Amotor, 2, 0); - - // These lowstops and high stops are effectively (no wiggle room) - d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); - d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); - 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, 9000f); - d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); - d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// - } - - private Matrix4 FromDMass(d.Mass pMass) - { - Matrix4 obj; - obj.M11 = pMass.I.M00; - obj.M12 = pMass.I.M01; - obj.M13 = pMass.I.M02; - obj.M14 = 0; - obj.M21 = pMass.I.M10; - obj.M22 = pMass.I.M11; - obj.M23 = pMass.I.M12; - obj.M24 = 0; - obj.M31 = pMass.I.M20; - obj.M32 = pMass.I.M21; - obj.M33 = pMass.I.M22; - obj.M34 = 0; - obj.M41 = 0; - obj.M42 = 0; - obj.M43 = 0; - obj.M44 = 1; - return obj; - } - - private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) - { - obj.I.M00 = pMat[0, 0]; - obj.I.M01 = pMat[0, 1]; - obj.I.M02 = pMat[0, 2]; - obj.I.M10 = pMat[1, 0]; - obj.I.M11 = pMat[1, 1]; - obj.I.M12 = pMat[1, 2]; - obj.I.M20 = pMat[2, 0]; - obj.I.M21 = pMat[2, 1]; - obj.I.M22 = pMat[2, 2]; - return obj; } public override void SubscribeEvents(int ms) @@ -3135,7 +3126,7 @@ Console.WriteLine(" JointCreateFixed"); m_eventsubscription = 0; } - public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { CollisionEventsThisFrame.AddCollider(CollidedWith, contact); } @@ -3165,173 +3156,6 @@ Console.WriteLine(" JointCreateFixed"); return false; } - public static Matrix4 Inverse(Matrix4 pMat) - { - if (determinant3x3(pMat) == 0) - { - return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible - } - - return (Adjoint(pMat) / determinant3x3(pMat)); - } - - public static Matrix4 Adjoint(Matrix4 pMat) - { - Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) - { - for (int j=0; j<4; j++) - { - Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); - } - } - - adjointMatrix = Transpose(adjointMatrix); - return adjointMatrix; - } - - public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) - { - Matrix4 minor = new Matrix4(); - int m = 0, n = 0; - for (int i = 0; i < 4; i++) - { - if (i == iRow) - continue; - n = 0; - for (int j = 0; j < 4; j++) - { - if (j == iCol) - continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); - n++; - } - m++; - } - - return minor; - } - - public static Matrix4 Transpose(Matrix4 pMat) - { - Matrix4 transposeMatrix = new Matrix4(); - for (int i = 0; i < 4; i++) - for (int j = 0; j < 4; j++) - Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); - return transposeMatrix; - } - - public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) - { - switch (r) - { - case 0: - switch (c) - { - case 0: - pMat.M11 = val; - break; - case 1: - pMat.M12 = val; - break; - case 2: - pMat.M13 = val; - break; - case 3: - pMat.M14 = val; - break; - } - - break; - case 1: - switch (c) - { - case 0: - pMat.M21 = val; - break; - case 1: - pMat.M22 = val; - break; - case 2: - pMat.M23 = val; - break; - case 3: - pMat.M24 = val; - break; - } - - break; - case 2: - switch (c) - { - case 0: - pMat.M31 = val; - break; - case 1: - pMat.M32 = val; - break; - case 2: - pMat.M33 = val; - break; - case 3: - pMat.M34 = val; - break; - } - - break; - case 3: - switch (c) - { - case 0: - pMat.M41 = val; - break; - case 1: - pMat.M42 = val; - break; - case 2: - pMat.M43 = val; - break; - case 3: - pMat.M44 = val; - break; - } - - break; - } - } - - 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]; - - 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; - 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; - } - public override void SetMaterial(int pMaterial) { m_material = pMaterial; @@ -3365,7 +3189,7 @@ Console.WriteLine(" JointCreateFixed"); // m_assetFailed = false; // m_log.DebugFormat( -// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", +// "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", // _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name); m_taintshape = true; @@ -3377,6 +3201,6 @@ Console.WriteLine(" JointCreateFixed"); "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}", _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName); } - } + } } } \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs index 8d610f7..78dd7de 100644 --- a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs +++ b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs @@ -32,7 +32,6 @@ using System.Runtime.InteropServices; using System.Text; using OpenMetaverse; using OpenSim.Region.PhysicsModules.SharedBase; -using Ode.NET; using log4net; namespace OpenSim.Region.PhysicsModule.ODE @@ -76,7 +75,7 @@ namespace OpenSim.Region.PhysicsModule.ODE { m_scene = pScene; nearCallback = near; - + } /// @@ -137,7 +136,7 @@ namespace OpenSim.Region.PhysicsModule.ODE ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); for (int i = 0; i < reqs.Length; i++) { - if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast RayCast(reqs[i]); // if there isn't anyone to send results } @@ -152,7 +151,7 @@ namespace OpenSim.Region.PhysicsModule.ODE ODERayRequest[] reqs = m_PendingRayRequests.ToArray(); for (int i = 0; i < reqs.Length; i++) { - if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast + if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast RayCast(reqs[i]); // if there isn't anyone to send results } @@ -176,11 +175,11 @@ namespace OpenSim.Region.PhysicsModule.ODE // this value may still be too large, depending on machine configuration // of maximum stack float len = req.length; - if (len > 250f) - len = 250f; + if (len > 100f) + len = 100f; // Create the ray - IntPtr ray = d.CreateRay(m_scene.space, req.length); + IntPtr ray = d.CreateRay(m_scene.space, len); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); // Collide test @@ -225,8 +224,13 @@ namespace OpenSim.Region.PhysicsModule.ODE /// private void RayCast(ODERayRequest req) { + // limit ray length or collisions will take all avaiable stack space + float len = req.length; + if (len > 100f) + len = 100f; + // Create the ray - IntPtr ray = d.CreateRay(m_scene.space, req.length); + IntPtr ray = d.CreateRay(m_scene.space, len); d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); // Collide test @@ -243,83 +247,22 @@ namespace OpenSim.Region.PhysicsModule.ODE req.callbackMethod(m_contactResults); } } - + // This is the standard Near. Uses space AABBs to speed up detection. private void near(IntPtr space, IntPtr g1, IntPtr g2) { - //Don't test against heightfield Geom, or you'll be sorry! - - /* - terminate called after throwing an instance of 'std::bad_alloc' - what(): std::bad_alloc - Stacktrace: - - at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004> - at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff> - at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280> - at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff - fffff> - at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004> - at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff> - at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) < - 0x00114> - at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb> - at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6> - at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042> - at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e> - at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019> - at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff> - - Native stacktrace: - - mono [0x80d2a42] - [0xb7f5840c] - /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018] - /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988] - /usr/lib/libstdc++.so.6 [0xb45fa865] - /usr/lib/libstdc++.so.6 [0xb45fa8a2] - /usr/lib/libstdc++.so.6 [0xb45fa9da] - /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033] - /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d] - libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4] - libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b] - libode.so(dCollide+0x102) [0xb46571b2] - [0x95cfdec9] - [0x8ea07fe1] - [0xab260146] - libode.so [0xb465a5c4] - libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5] - libode.so(dSpaceCollide2+0x177) [0xb465ac67] - [0x95cf978e] - [0x8ea07945] - [0x95cf2bbc] - [0xab2787e7] - [0xab419fb3] - [0xab416657] - [0xab415bda] - [0xb609b08e] - mono(mono_runtime_delegate_invoke+0x34) [0x8192534] - mono [0x81a2f0f] - mono [0x81d28b6] - mono [0x81ea2c6] - /lib/i686/cmov/libpthread.so.0 [0xb7e744c0] - /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de] - */ - - // Exclude heightfield geom - if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) return; - if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass) - return; +// if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass) +// return; // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) { if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) return; - + // Separating static prim geometry spaces. // We'll be calling near recursivly if one // of them is a space to find all of the @@ -347,13 +290,13 @@ namespace OpenSim.Region.PhysicsModule.ODE int count = 0; try { - + if (g1 == g2) return; // Can't collide with yourself lock (contacts) { - count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf); + count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.unmanagedSizeOf); } } catch (SEHException) @@ -383,7 +326,7 @@ namespace OpenSim.Region.PhysicsModule.ODE if (p1 is OdePrim) { ContactResult collisionresult = new ContactResult(); - + collisionresult.ConsumerID = p1.LocalID; collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z); collisionresult.Depth = contacts[i].depth; @@ -438,4 +381,4 @@ namespace OpenSim.Region.PhysicsModule.ODE public float length; public RayCallback callbackMethod; } -} \ No newline at end of file +} diff --git a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs index 2eb7ba6..e2578be 100644 --- a/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs +++ b/OpenSim/Region/PhysicsModules/Ode/OdePhysicsJoint.cs @@ -27,10 +27,8 @@ using System; using OpenMetaverse; -using Ode.NET; using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; -using OpenSim.Region.PhysicsModule.ODE; namespace OpenSim.Region.PhysicsModule.ODE { diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs index 8cc7f28..d15568e 100644 --- a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs +++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs @@ -31,31 +31,24 @@ // or application thread stack may just blowup // see RayCast(ODERayCastRequest req) -//#define USE_DRAWSTUFF -//#define SPAM - using System; using System.Collections.Generic; using System.Diagnostics; using System.IO; using System.Linq; using System.Reflection; +using System.Runtime.ExceptionServices; using System.Runtime.InteropServices; using System.Threading; using log4net; using Nini.Config; using Mono.Addins; -using Ode.NET; using OpenMetaverse; -#if USE_DRAWSTUFF -using Drawstuff.NET; -#endif using OpenSim.Framework; using OpenSim.Region.PhysicsModules.SharedBase; using OpenSim.Region.Framework.Scenes; using OpenSim.Region.Framework.Interfaces; - namespace OpenSim.Region.PhysicsModule.ODE { public enum StatusIndicators : int @@ -65,16 +58,6 @@ namespace OpenSim.Region.PhysicsModule.ODE 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 { @@ -111,11 +94,9 @@ namespace OpenSim.Region.PhysicsModule.ODE Rubber = 6 } - [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ODEPhysicsScene")] - public class OdeScene : PhysicsScene, INonSharedRegionModule + public class OdeScene : PhysicsScene { private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString()); - private bool m_Enabled = false; // private Dictionary m_storedCollisions = new Dictionary(); @@ -144,6 +125,7 @@ namespace OpenSim.Region.PhysicsModule.ODE /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82] /// internal static Object UniversalColliderSyncObject = new Object(); + internal static Object SimulationLock = new Object(); /// /// Is stats collecting enabled for this ODE scene? @@ -276,8 +258,6 @@ namespace OpenSim.Region.PhysicsModule.ODE private Random fluidRandomizer = new Random(Environment.TickCount); - public bool m_suportCombine = true; - private uint m_regionWidth = Constants.RegionSize; private uint m_regionHeight = Constants.RegionSize; @@ -293,11 +273,8 @@ namespace OpenSim.Region.PhysicsModule.ODE private float contactsurfacelayer = 0.001f; - private int worldHashspaceLow = -4; - private int worldHashspaceHigh = 128; - - private int smallHashspaceLow = -4; - private int smallHashspaceHigh = 66; + private int HashspaceLow = -5; + private int HashspaceHigh = 12; private float waterlevel = 0f; private int framecount = 0; @@ -335,7 +312,6 @@ namespace OpenSim.Region.PhysicsModule.ODE public bool IsAvCapsuleTilted { get; private set; } private float avDensity = 80f; -// private float avHeightFudgeFactor = 0.52f; private float avMovementDivisorWalk = 1.3f; private float avMovementDivisorRun = 0.8f; private float minimumGroundFlightOffset = 3f; @@ -356,13 +332,8 @@ namespace OpenSim.Region.PhysicsModule.ODE public float bodyPIDD = 35f; public float bodyPIDG = 25; - public int geomCrossingFailuresBeforeOutofbounds = 5; - - public float bodyMotorJointMaxforceTensor = 2; - public int bodyFramesAutoDisable = 20; - private float[] _watermap; private bool m_filterCollisions = true; private d.NearCallback nearCallback; @@ -473,31 +444,21 @@ namespace OpenSim.Region.PhysicsModule.ODE private d.Contact WaterContact; private d.Contact[,] m_materialContacts; -//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it -//Ckrinke private int m_randomizeWater = 200; 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; -//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it -//Ckrinke private int ms = 0; + private float step_time = 0.0f; public IntPtr world; - //private bool returncollisions = false; - // private uint obj1LocalID = 0; private uint obj2LocalID = 0; - //private int ctype = 0; private OdeCharacter cc1; private OdePrim cp1; private OdeCharacter cc2; private OdePrim cp2; private int p1ExpectedPoints = 0; private int p2ExpectedPoints = 0; - //private int cStartStop = 0; - //private string cDictKey = ""; public IntPtr space; - //private IntPtr tmpSpace; // split static geometry collision handling into spaces of 30 meters public IntPtr[,] staticPrimspace; @@ -535,88 +496,37 @@ namespace OpenSim.Region.PhysicsModule.ODE private ODERayCastRequestManager m_rayCastManager; + public Scene m_frameWorkScene = null; - #region INonSharedRegionModule - public string Name - { - get { return "OpenDynamicsEngine"; } - } - - public Type ReplaceableInterface - { - get { return null; } - } - - public void Initialise(IConfigSource source) - { - // TODO: Move this out of Startup - IConfig config = source.Configs["Startup"]; - if (config != null) - { - string physics = config.GetString("physics", string.Empty); - if (physics == Name) - { - m_Enabled = true; - m_config = source; - - // We do this so that OpenSimulator on Windows loads the correct native ODE library depending on whether - // it's running as a 32-bit process or a 64-bit one. By invoking LoadLibary here, later DLLImports - // will find it already loaded later on. - // - // This isn't necessary for other platforms (e.g. Mac OSX and Linux) since the DLL used can be - // controlled in Ode.NET.dll.config - if (Util.IsWindows()) - Util.LoadArchSpecificWindowsDll("ode.dll"); - - // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to - // http://opensimulator.org/mantis/view.php?id=2750). - d.InitODE(); - - } - } - - } - - public void Close() - { - } - - public void AddRegion(Scene scene) + public OdeScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion) { - if (!m_Enabled) - return; + m_config = psourceconfig; + m_frameWorkScene = pscene; - EngineType = Name; - PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName; + EngineType = pname; + PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName; + EngineName = pname + " " + pversion; - scene.RegisterModuleInterface(this); - Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ); + pscene.RegisterModuleInterface(this); + Vector3 extent = new Vector3(pscene.RegionInfo.RegionSizeX, pscene.RegionInfo.RegionSizeY, pscene.RegionInfo.RegionSizeZ); Initialise(extent); InitialiseFromConfig(m_config); // This may not be that good since terrain may not be avaiable at this point - base.Initialise(scene.PhysicsRequestAsset, - (scene.Heightmap != null ? scene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]), - (float)scene.RegionInfo.RegionSettings.WaterHeight); + base.Initialise(pscene.PhysicsRequestAsset, + (pscene.Heightmap != null ? pscene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]), + (float)pscene.RegionInfo.RegionSettings.WaterHeight); } - public void RemoveRegion(Scene scene) + public void RegionLoaded() { - if (!m_Enabled) - return; - } - - public void RegionLoaded(Scene scene) - { - if (!m_Enabled) - return; - - mesher = scene.RequestModuleInterface(); + mesher = m_frameWorkScene.RequestModuleInterface(); if (mesher == null) m_log.WarnFormat("[ODE SCENE]: No mesher in {0}. Things will not work well.", PhysicsSceneName); + + m_frameWorkScene.PhysicsEnabled = true; } - #endregion /// /// Initiailizes the scene @@ -625,16 +535,12 @@ namespace OpenSim.Region.PhysicsModule.ODE /// private void Initialise(Vector3 regionExtent) { - WorldExtents.X = regionExtent.X; + WorldExtents.X = regionExtent.X; m_regionWidth = (uint)regionExtent.X; WorldExtents.Y = regionExtent.Y; m_regionHeight = (uint)regionExtent.Y; - - m_suportCombine = false; - + nearCallback = near; - triCallback = TriCallback; - triArrayCallback = TriArrayCallback; m_rayCastManager = new ODERayCastRequestManager(this); // Create the world and the first space @@ -644,33 +550,8 @@ namespace OpenSim.Region.PhysicsModule.ODE contactgroup = d.JointGroupCreate(0); d.WorldSetAutoDisableFlag(world, false); - - #if USE_DRAWSTUFF - Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); - viewthread.Start(); - #endif - - // _watermap = new float[258 * 258]; - - // Zero out the prim spaces array (we split our space into smaller spaces so - // we can hit test less. } -#if USE_DRAWSTUFF - public void startvisualization(object o) - { - ds.Functions fn; - fn.version = ds.VERSION; - fn.start = new ds.CallbackFunction(start); - fn.step = new ds.CallbackFunction(step); - fn.command = new ds.CallbackFunction(command); - fn.stop = null; - fn.path_to_textures = "./textures"; - string[] args = new string[0]; - ds.SimulationLoop(args.Length, args, 352, 288, ref fn); - } -#endif - // Initialize from configs private void InitialiseFromConfig(IConfigSource config) { @@ -679,18 +560,9 @@ namespace OpenSim.Region.PhysicsModule.ODE m_config = config; // Defaults - if (Environment.OSVersion.Platform == PlatformID.Unix) - { - avPIDD = 3200.0f; - avPIDP = 1400.0f; - avStandupTensor = 2000000f; - } - else - { - avPIDD = 2200.0f; - avPIDP = 900.0f; - avStandupTensor = 550000f; - } + avPIDD = 2200.0f; + avPIDP = 900.0f; + avStandupTensor = 550000f; int contactsPerCollision = 80; @@ -714,12 +586,10 @@ namespace OpenSim.Region.PhysicsModule.ODE avatarTerminalVelocity, AvatarTerminalVelocity); } - worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4); - worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128); + HashspaceLow = physicsconfig.GetInt("world_hashspace_level_low", -5); + HashspaceHigh = physicsconfig.GetInt("world_hashspace_level_high", 12); metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); - smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4); - smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66); contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f); @@ -738,7 +608,7 @@ namespace OpenSim.Region.PhysicsModule.ODE mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f); ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); - m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + m_physicsiterations = physicsconfig.GetInt("world_solver_iterations", 10); avDensity = physicsconfig.GetFloat("av_density", 80f); // avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); @@ -754,7 +624,6 @@ namespace OpenSim.Region.PhysicsModule.ODE geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); 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); @@ -767,29 +636,16 @@ namespace OpenSim.Region.PhysicsModule.ODE 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); - avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f); - bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f); - } - else - { - avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); - avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); - avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f); - bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f); - } + avPIDD = physicsconfig.GetFloat("av_pid_derivative", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional", 900.0f); + avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor", 550000f); 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); - m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); +// m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); } @@ -812,17 +668,15 @@ namespace OpenSim.Region.PhysicsModule.ODE if (spaceGridMaxY > 24) { spaceGridMaxY = 24; - spacesPerMeterY = spaceGridMaxY / WorldExtents.Y ; + spacesPerMeterY = spaceGridMaxY / WorldExtents.Y; } - staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; // make this index limits spaceGridMaxX--; spaceGridMaxY--; - - // Centeral contact friction and bounce // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why // an avatar falls through in Z but not in X or Y when walking on a prim. @@ -984,7 +838,7 @@ namespace OpenSim.Region.PhysicsModule.ODE m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f; m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f; - d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh); + d.HashSpaceSetLevels(space, HashspaceLow, HashspaceHigh); // Set the gravity,, don't disable things automatically (we set it explicitly on some things) @@ -997,9 +851,6 @@ namespace OpenSim.Region.PhysicsModule.ODE d.WorldSetLinearDampingThreshold(world, 256f); d.WorldSetMaxAngularSpeed(world, 256f); - // 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, 1000.0f); @@ -1014,22 +865,6 @@ namespace OpenSim.Region.PhysicsModule.ODE _worldInitialized = true; } -// internal void waitForSpaceUnlock(IntPtr space) -// { -// //if (space != IntPtr.Zero) -// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing -// } - -// /// -// /// Debug space message for printing the space that a prim/avatar is in. -// /// -// /// -// /// Returns which split up space the given position is in. -// public string whichspaceamIin(Vector3 pos) -// { -// return calculateSpaceForGeom(pos).ToString(); -// } - #region Collision Detection /// @@ -1042,7 +877,7 @@ namespace OpenSim.Region.PhysicsModule.ODE /// /// private int CollideGeoms( - IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize) + IntPtr geom1, IntPtr geom2, int maxContacts, d.ContactGeom[] contactsArray, int contactGeomSize) { int count; @@ -1113,7 +948,7 @@ namespace OpenSim.Region.PhysicsModule.ODE { if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) return; - + // Separating static prim geometry spaces. // We'll be calling near recursivly if one // of them is a space to find all of the @@ -1155,12 +990,6 @@ namespace OpenSim.Region.PhysicsModule.ODE name2 = "null"; } - //if (id == d.GeomClassId.TriMeshClass) - //{ - // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2); - //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2); - //} - // Figure out how many contact points we have int count = 0; @@ -1175,7 +1004,7 @@ namespace OpenSim.Region.PhysicsModule.ODE if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) return; - count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); + count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.unmanagedSizeOf); // All code after this is only relevant if we have any collisions if (count <= 0) @@ -1198,10 +1027,10 @@ namespace OpenSim.Region.PhysicsModule.ODE PhysicsActor p1; PhysicsActor p2; - + p1ExpectedPoints = 0; p2ExpectedPoints = 0; - + if (!actor_name_map.TryGetValue(g1, out p1)) { p1 = PANull; @@ -1238,7 +1067,7 @@ namespace OpenSim.Region.PhysicsModule.ODE IntPtr joint; // If we're colliding with terrain, use 'TerrainContact' instead of contact. // allows us to have different settings - + // We only need to test p2 for 'jump crouch purposes' if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim) { @@ -1252,7 +1081,7 @@ namespace OpenSim.Region.PhysicsModule.ODE { p2.IsColliding = true; } - + //if ((framecount % m_returncollisions) == 0) switch (p1.PhysicsActorType) @@ -1279,110 +1108,9 @@ namespace OpenSim.Region.PhysicsModule.ODE // we don't want prim or avatar to explode #region InterPenetration Handling - Unintended physics explosions -# region disabled code1 if (curContact.depth >= 0.08f) { - //This is disabled at the moment only because it needs more tweaking - //It will eventually be uncommented - /* - if (contact.depth >= 1.00f) - { - //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); - } - - //If you interpenetrate a prim with an agent - if ((p2.PhysicsActorType == (int) ActorTypes.Agent && - p1.PhysicsActorType == (int) ActorTypes.Prim) || - (p1.PhysicsActorType == (int) ActorTypes.Agent && - p2.PhysicsActorType == (int) ActorTypes.Prim)) - { - - //contact.depth = contact.depth * 4.15f; - /* - if (p2.PhysicsActorType == (int) ActorTypes.Agent) - { - p2.CollidingObj = true; - contact.depth = 0.003f; - p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); - OdeCharacter character = (OdeCharacter) p2; - character.SetPidStatus(true); - contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2)); - - } - else - { - - //contact.depth = 0.0000000f; - } - if (p1.PhysicsActorType == (int) ActorTypes.Agent) - { - - p1.CollidingObj = true; - contact.depth = 0.003f; - p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f); - contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2)); - OdeCharacter character = (OdeCharacter)p1; - character.SetPidStatus(true); - } - else - { - - //contact.depth = 0.0000000f; - } - - - - } -*/ - // If you interpenetrate a prim with another prim - /* - if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim) - { - #region disabledcode2 - //OdePrim op1 = (OdePrim)p1; - //OdePrim op2 = (OdePrim)p2; - //op1.m_collisionscore++; - //op2.m_collisionscore++; - - //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000) - //{ - //op1.m_taintdisable = true; - //AddPhysicsActorTaint(p1); - //op2.m_taintdisable = true; - //AddPhysicsActorTaint(p2); - //} - - //if (contact.depth >= 0.25f) - //{ - // Don't collide, one or both prim will expld. - - //op1.m_interpenetrationcount++; - //op2.m_interpenetrationcount++; - //interpenetrations_before_disable = 200; - //if (op1.m_interpenetrationcount >= interpenetrations_before_disable) - //{ - //op1.m_taintdisable = true; - //AddPhysicsActorTaint(p1); - //} - //if (op2.m_interpenetrationcount >= interpenetrations_before_disable) - //{ - // op2.m_taintdisable = true; - //AddPhysicsActorTaint(p2); - //} - - //contact.depth = contact.depth / 8f; - //contact.normal = new d.Vector3(0, 0, 1); - //} - //if (op1.m_disabled || op2.m_disabled) - //{ - //Manually disabled objects stay disabled - //contact.depth = 0f; - //} - #endregion - } - */ -#endregion if (curContact.depth >= 1.00f) { //m_log.Info("[P]: " + contact.depth.ToString()); @@ -1432,7 +1160,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // Logic for collision handling // Note, that if *all* contacts are skipped (VolumeDetect) - // The prim still detects (and forwards) collision events but + // The prim still detects (and forwards) collision events but // appears to be phantom for the world Boolean skipThisContact = false; @@ -1507,10 +1235,10 @@ namespace OpenSim.Region.PhysicsModule.ODE material = ((OdePrim) p2).m_material; p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; } - + // Unnessesary because p1 is defined above //if (p1 is OdePrim) - // { + // { // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts; // } //m_log.DebugFormat("Material: {0}", material); @@ -1622,7 +1350,7 @@ namespace OpenSim.Region.PhysicsModule.ODE material = ((OdePrim)p2).m_material; p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; } - + //m_log.DebugFormat("Material: {0}", material); m_materialContacts[material, 0].geom = curContact; @@ -1642,7 +1370,7 @@ namespace OpenSim.Region.PhysicsModule.ODE } collision_accounting_events(p1, p2, maxDepthContact); - + if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle)) { // If there are more then 3 contact points, it's likely @@ -1678,31 +1406,13 @@ namespace OpenSim.Region.PhysicsModule.ODE { if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f) { - //contactGeom.depth *= .00005f; - //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); - // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); - result = true; - break; + result = true; + break; } -// else -// { -// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth)); -// } } -// else -// { -// //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z)); -// //int i = 0; -// } - } + } else if (at == ActorTypes.Prim) { - //d.AABB aabb1 = new d.AABB(); - //d.AABB aabb2 = new d.AABB(); - - //d.GeomGetAABB(contactGeom.g2, out aabb2); - //d.GeomGetAABB(contactGeom.g1, out aabb1); - //aabb1. if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f))) { if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) @@ -1729,9 +1439,28 @@ namespace OpenSim.Region.PhysicsModule.ODE obj2LocalID = 0; //ctype = 0; //cStartStop = 0; - if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) +// if (!p2.SubscribedEvents() && !p1.SubscribedEvents()) +// return; + bool p1events = p1.SubscribedEvents(); + bool p2events = p2.SubscribedEvents(); + + if (p1.IsVolumeDtc) + p2events = false; + if (p2.IsVolumeDtc) + p1events = false; + + if (!p2events && !p1events) return; + Vector3 vel = Vector3.Zero; + if (p2 != null && p2.IsPhysical) + vel = p2.Velocity; + + if (p1 != null && p1.IsPhysical) + vel -= p1.Velocity; + + contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + switch ((ActorTypes)p2.PhysicsActorType) { case ActorTypes.Agent: @@ -1744,14 +1473,6 @@ namespace OpenSim.Region.PhysicsModule.ODE cc1 = (OdeCharacter)p1; obj2LocalID = cc1.LocalID; cc1.AddCollisionEvent(cc2.LocalID, contact); - //ctype = (int)CollisionCategories.Character; - - //if (cc1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - - //returncollisions = true; break; case ActorTypes.Prim: @@ -1761,21 +1482,11 @@ namespace OpenSim.Region.PhysicsModule.ODE obj2LocalID = cp1.LocalID; cp1.AddCollisionEvent(cc2.LocalID, contact); } - //ctype = (int)CollisionCategories.Geom; - - //if (cp1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - - //returncollisions = true; break; case ActorTypes.Ground: case ActorTypes.Unknown: obj2LocalID = 0; - //ctype = (int)CollisionCategories.Land; - //returncollisions = true; break; } @@ -1797,13 +1508,6 @@ namespace OpenSim.Region.PhysicsModule.ODE cc1 = (OdeCharacter) p1; obj2LocalID = cc1.LocalID; cc1.AddCollisionEvent(cp2.LocalID, contact); - //ctype = (int)CollisionCategories.Character; - - //if (cc1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - //returncollisions = true; } break; case ActorTypes.Prim: @@ -1813,23 +1517,12 @@ namespace OpenSim.Region.PhysicsModule.ODE cp1 = (OdePrim) p1; obj2LocalID = cp1.LocalID; cp1.AddCollisionEvent(cp2.LocalID, contact); - //ctype = (int)CollisionCategories.Geom; - - //if (cp1.CollidingObj) - //cStartStop = (int)StatusIndicators.Generic; - //else - //cStartStop = (int)StatusIndicators.Start; - - //returncollisions = true; } break; case ActorTypes.Ground: case ActorTypes.Unknown: obj2LocalID = 0; - //ctype = (int)CollisionCategories.Land; - - //returncollisions = true; break; } @@ -1837,80 +1530,7 @@ namespace OpenSim.Region.PhysicsModule.ODE } break; } - //if (returncollisions) - //{ - - //lock (m_storedCollisions) - //{ - //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString(); - //if (m_storedCollisions.ContainsKey(cDictKey)) - //{ - //sCollisionData objd = m_storedCollisions[cDictKey]; - //objd.NumberOfCollisions += 1; - //objd.lastframe = framecount; - //m_storedCollisions[cDictKey] = objd; - //} - //else - //{ - //sCollisionData objd = new sCollisionData(); - //objd.ColliderLocalId = obj1LocalID; - //objd.CollidedWithLocalId = obj2LocalID; - //objd.CollisionType = ctype; - //objd.NumberOfCollisions = 1; - //objd.lastframe = framecount; - //objd.StatusIndicator = cStartStop; - //m_storedCollisions.Add(cDictKey, objd); - //} - //} - // } - } - - private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount) - { - /* String name1 = null; - String name2 = null; - - if (!geom_name_map.TryGetValue(trimesh, out name1)) - { - name1 = "null"; - } - if (!geom_name_map.TryGetValue(refObject, out name2)) - { - name2 = "null"; - } - - m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); - */ - return 1; - } - - private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex) - { -// String name1 = null; -// String name2 = null; -// -// if (!geom_name_map.TryGetValue(trimesh, out name1)) -// { -// name1 = "null"; -// } -// -// if (!geom_name_map.TryGetValue(refObject, out name2)) -// { -// name2 = "null"; -// } - - // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex); - - d.Vector3 v0 = new d.Vector3(); - d.Vector3 v1 = new d.Vector3(); - d.Vector3 v2 = new d.Vector3(); - - d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2); - // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z); - - return 1; } - /// /// This is our collision testing routine in ODE /// @@ -1924,11 +1544,11 @@ namespace OpenSim.Region.PhysicsModule.ODE // since we don't know if we're colliding yet if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) continue; - + chr.IsColliding = false; chr.CollidingGround = false; chr.CollidingObj = false; - + // Test the avatar's geometry for collision with the space // This will return near and the space that they are the closest to // And we'll run this again against the avatar and the space segment @@ -1942,7 +1562,7 @@ namespace OpenSim.Region.PhysicsModule.ODE { m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", PhysicsSceneName); } - + //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y); //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10) //{ @@ -2003,15 +1623,6 @@ namespace OpenSim.Region.PhysicsModule.ODE #endregion - public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) - { - if (!m_suportCombine) - return; - m_worldOffset = offset; - WorldExtents = new Vector2(extents.X, extents.Y); - m_parentScene = pScene; - } - // Recovered for use by fly height. Kitto Flora internal float GetTerrainHeightAtXY(float x, float y) { @@ -2019,12 +1630,6 @@ namespace OpenSim.Region.PhysicsModule.ODE int offsetX = 0; int offsetY = 0; - if (m_suportCombine) - { - offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; - } - if(RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom)) { if (heightFieldGeom != IntPtr.Zero) @@ -2042,14 +1647,15 @@ namespace OpenSim.Region.PhysicsModule.ODE x = x - offsetX + 1f; y = y - offsetY + 1f; - index = (int)((int)x * ((int)m_regionHeight +3) + (int)y); + // map is rotated + index = (int)x * ((int)m_regionHeight + 3) + (int)y; if (index < TerrainHeightFieldHeights[heightFieldGeom].Length) { //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]); return (float)TerrainHeightFieldHeights[heightFieldGeom][index]; } - + else return 0f; } @@ -2069,7 +1675,7 @@ namespace OpenSim.Region.PhysicsModule.ODE { return 0f; } - } + } // End recovered. Kitto Flora /// @@ -2079,7 +1685,7 @@ namespace OpenSim.Region.PhysicsModule.ODE internal void AddCollisionEventReporting(PhysicsActor obj) { // m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID); - + lock (m_collisionEventActorsChanges) m_collisionEventActorsChanges[obj.LocalID] = obj; } @@ -2099,7 +1705,9 @@ namespace OpenSim.Region.PhysicsModule.ODE #region Add/Remove Entities public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) - { + { + d.AllocateODEDataForThread(0); + OdeCharacter newAv = new OdeCharacter( avName, this, position, velocity, size, avPIDD, avPIDP, @@ -2119,7 +1727,12 @@ namespace OpenSim.Region.PhysicsModule.ODE // "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}", // actor.Name, actor.LocalID, Name); - ((OdeCharacter) actor).Destroy(); + lock (OdeLock) + { + d.AllocateODEDataForThread(0); + + ((OdeCharacter) actor).Destroy(); + } } internal void AddCharacter(OdeCharacter chr) @@ -2169,9 +1782,11 @@ namespace OpenSim.Region.PhysicsModule.ODE Vector3 siz = size; Quaternion rot = rotation; + OdePrim newPrim; lock (OdeLock) { + d.AllocateODEDataForThread(0); newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); lock (_prims) @@ -2456,7 +2071,7 @@ namespace OpenSim.Region.PhysicsModule.ODE lock (externalJointRequestsLock) { - if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice + if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice { requestedJointsToBeCreated.Add(joint); } @@ -2620,59 +2235,13 @@ namespace OpenSim.Region.PhysicsModule.ODE } - // we don't want to remove the main space - - // If the geometry is in the targetspace, remove it from the target space - //m_log.Warn(prim.m_targetSpace); - - //if (prim.m_targetSpace != IntPtr.Zero) - //{ - //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom)) - //{ - - //if (d.GeomIsSpace(prim.m_targetSpace)) - //{ - //waitForSpaceUnlock(prim.m_targetSpace); - //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom); prim.m_targetSpace = IntPtr.Zero; - //} - //else - //{ - // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + - //((OdePrim)prim).m_targetSpace.ToString()); - //} - - //} - //} - //m_log.Warn(prim.prim_geom); - if (!prim.RemoveGeom()) m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene"); lock (_prims) _prims.Remove(prim); - //If there are no more geometries in the sub-space, we don't need it in the main space anymore - //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0) - //{ - //if (prim.m_targetSpace != null) - //{ - //if (d.GeomIsSpace(prim.m_targetSpace)) - //{ - //waitForSpaceUnlock(prim.m_targetSpace); - //d.SpaceRemove(space, prim.m_targetSpace); - // free up memory used by the space. - //d.SpaceDestroy(prim.m_targetSpace); - //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position); - //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]); - //} - //else - //{ - //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" + - //((OdePrim) prim).m_targetSpace.ToString()); - //} - //} - //} if (SupportsNINJAJoints) RemoveAllJointsConnectedToActorThreadLocked(prim); @@ -2768,12 +2337,9 @@ namespace OpenSim.Region.PhysicsModule.ODE { if (d.GeomIsSpace(currentspace)) { -// waitForSpaceUnlock(currentspace); -// waitForSpaceUnlock(space); d.SpaceRemove(space, currentspace); // free up memory used by the space. - //d.SpaceDestroy(currentspace); resetSpaceArrayItemToZero(currentspace); } else @@ -2831,7 +2397,7 @@ namespace OpenSim.Region.PhysicsModule.ODE if (newspace == IntPtr.Zero) { newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); - d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh); + d.HashSpaceSetLevels(newspace, HashspaceLow, HashspaceHigh); } return newspace; @@ -2851,7 +2417,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // waitForSpaceUnlock(space); d.SpaceSetSublevel(space, 1); d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]); - + return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]; } @@ -2948,7 +2514,7 @@ namespace OpenSim.Region.PhysicsModule.ODE iPropertiesNotSupportedDefault++; if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) - iPropertiesNotSupportedDefault++; + iPropertiesNotSupportedDefault++; if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) iPropertiesNotSupportedDefault++; @@ -3021,7 +2587,7 @@ namespace OpenSim.Region.PhysicsModule.ODE #if SPAM m_log.Debug("Mesh"); #endif - return true; + return true; } /// @@ -3052,6 +2618,49 @@ namespace OpenSim.Region.PhysicsModule.ODE } } + // does all pending changes generated during region load process + public override void ProcessPreSimulation() + { + lock (OdeLock) + { + if (world == IntPtr.Zero) + { + _taintedPrims.Clear();; + return; + } + + int donechanges = 0; + if (_taintedPrims.Count > 0) + { + + m_log.InfoFormat("[Ode] start processing pending actor operations"); + int tstart = Util.EnvironmentTickCount(); + + d.AllocateODEDataForThread(0); + + lock (_taintedPrims) + { + foreach (OdePrim prim in _taintedPrims) + { + if (prim.m_taintremove) + RemovePrimThreadLocked(prim); + else + prim.ProcessTaints(); + + prim.m_collisionscore = 0; + donechanges++; + } + _taintedPrims.Clear(); + } + + int time = Util.EnvironmentTickCountSubtract(tstart); + m_log.InfoFormat("[Ode] finished {0} operations in {1}ms", donechanges, time); + } + m_log.InfoFormat("[Ode] {0} prim actors loaded",_prims.Count); + } + } + + /// /// This is our main simulate loop /// @@ -3065,7 +2674,8 @@ namespace OpenSim.Region.PhysicsModule.ODE /// The number of frames simulated over that period. public override float Simulate(float timeStep) { - if (!_worldInitialized) return 11f; + if (!_worldInitialized) + return 1.0f; int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0; int tempTick = 0, tempTick2 = 0; @@ -3077,26 +2687,12 @@ namespace OpenSim.Region.PhysicsModule.ODE float fps = 0; - float timeLeft = timeStep; - - //m_log.Info(timeStep.ToString()); -// step_time += timeSte -// -// // If We're loaded down by something else, -// // or debugging with the Visual Studio project on pause -// // skip a few frames to catch up gracefully. -// // without shooting the physicsactors all over the place -// -// if (step_time >= m_SkipFramesAtms) -// { -// // Instead of trying to catch up, it'll do 5 physics frames only -// step_time = ODE_STEPSIZE; -// m_physicsiterations = 5; -// } -// else -// { -// m_physicsiterations = 10; -// } + step_time += timeStep; + + float HalfOdeStep = ODE_STEPSIZE * 0.5f; + if (step_time < HalfOdeStep) + return 0; + // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential // deadlock if the collision event tries to lock something else later on which is already locked by a @@ -3120,29 +2716,12 @@ namespace OpenSim.Region.PhysicsModule.ODE CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks } + lock (OdeLock) { - // Process 10 frames if the sim is running normal.. - // process 5 frames if the sim is running slow - //try - //{ - //d.WorldSetQuickStepNumIterations(world, m_physicsiterations); - //} - //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(); - //} + d.AllocateODEDataForThread(~0U); - // Figure out the Frames Per Second we're going at. - //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size - - fps = (timeStep / ODE_STEPSIZE) * 1000; - // HACK: Using a time dilation of 1.0 to debug rubberbanding issues - //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f); - - while (timeLeft > 0.0f) + while (step_time > HalfOdeStep) { try { @@ -3241,10 +2820,6 @@ namespace OpenSim.Region.PhysicsModule.ODE tempTick = tempTick2; } - //if ((framecount % m_randomizeWater) == 0) - // randomizeWater(waterlevel); - - //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests(); m_rayCastManager.ProcessQueuedRequests(); if (CollectStats) @@ -3265,7 +2840,7 @@ namespace OpenSim.Region.PhysicsModule.ODE foreach (PhysicsActor obj in m_collisionEventActors.Values) { -// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); + // m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); switch ((ActorTypes)obj.PhysicsActorType) { @@ -3295,7 +2870,8 @@ namespace OpenSim.Region.PhysicsModule.ODE tempTick = tempTick2; } - d.WorldQuickStep(world, ODE_STEPSIZE); + lock(SimulationLock) + d.WorldQuickStep(world, ODE_STEPSIZE); if (CollectStats) m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); @@ -3307,7 +2883,8 @@ namespace OpenSim.Region.PhysicsModule.ODE m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e); } - timeLeft -= ODE_STEPSIZE; + step_time -= ODE_STEPSIZE; + fps += ODE_STEPSIZE; } if (CollectStats) @@ -3403,6 +2980,7 @@ namespace OpenSim.Region.PhysicsModule.ODE m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick); } + fps *= 1.0f/timeStep; return fps; } @@ -3531,7 +3109,7 @@ namespace OpenSim.Region.PhysicsModule.ODE // as the axis for the hinge. // Therefore, we must get the joint's coordinate frame based on the - // joint.Rotation field, which originates from the orientation of the + // joint.Rotation field, which originates from the orientation of the // joint's proxy object in the scene. // The joint's coordinate frame is defined as the transformation matrix @@ -3659,7 +3237,7 @@ namespace OpenSim.Region.PhysicsModule.ODE int startTime = Util.EnvironmentTickCount(); m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset); - + float[] _heightmap; // ok im lasy this are just a aliases @@ -3675,7 +3253,6 @@ namespace OpenSim.Region.PhysicsModule.ODE _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; - const float scale = 1.0f; const float offset = 0.0f; const float thickness = 10f; @@ -3722,6 +3299,8 @@ namespace OpenSim.Region.PhysicsModule.ODE lock (OdeLock) { + d.AllocateODEDataForThread(~0U); + IntPtr GroundGeom = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) { @@ -3740,7 +3319,8 @@ namespace OpenSim.Region.PhysicsModule.ODE IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth, heightmapHeight, - (int)heightmapWidthSamples, (int)heightmapHeightSamples, + (int)heightmapWidthSamples, + (int)heightmapHeightSamples, scale, offset, thickness, wrap); d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); @@ -3765,7 +3345,7 @@ namespace OpenSim.Region.PhysicsModule.ODE d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); d.GeomSetRotation(GroundGeom, ref R); - d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0f); + d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0.0f); IntPtr testGround = IntPtr.Zero; if (RegionTerrain.TryGetValue(pOffset, out testGround)) { @@ -3788,82 +3368,30 @@ namespace OpenSim.Region.PhysicsModule.ODE return waterlevel; } - public override bool SupportsCombining() - { - return m_suportCombine; - } - public override void SetWaterLevel(float baseheight) { waterlevel = baseheight; -// randomizeWater(waterlevel); } -/* - private void randomizeWater(float baseheight) + [HandleProcessCorruptedStateExceptions] + public override void Dispose() { - uint heightmapWidth = m_regionWidth + 2; - uint heightmapHeight = m_regionHeight + 2; - uint heightmapWidthSamples = m_regionWidth + 2; - uint heightmapHeightSamples = m_regionHeight + 2; - float scale = 1.0f; - float offset = 0.0f; - float thickness = 2.9f; - 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) + lock(SimulationLock) + lock(OdeLock) { - if (WaterGeom != IntPtr.Zero) - { - d.SpaceRemove(space, 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(space, 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); - //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); + if(world == IntPtr.Zero) + return; - q1 = q1 * q2; - //q1 = q1 * q3; - Vector3 v3; - float angle; - q1.GetAxisAngle(out v3, out angle); + _worldInitialized = false; - 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() - { - _worldInitialized = false; + d.AllocateODEDataForThread(~0U); - m_rayCastManager.Dispose(); - m_rayCastManager = null; + if (m_rayCastManager != null) + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + } - lock (OdeLock) - { lock (_prims) { foreach (OdePrim prm in _prims) @@ -3876,10 +3404,33 @@ namespace OpenSim.Region.PhysicsModule.ODE //{ //RemoveAvatar(act); //} - d.WorldDestroy(world); - //d.CloseODE(); + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(m_worldOffset, out GroundGeom)) + { + RegionTerrain.Remove(m_worldOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + TerrainHeightFieldHeights.Remove(GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + + try + { + d.WorldDestroy(world); + world = IntPtr.Zero; + } + catch (AccessViolationException e) + { + m_log.ErrorFormat("[ODE SCENE]: exception {0}", e.Message); + } } + } + private int compareByCollisionsDesc(OdePrim A, OdePrim B) + { + return -A.CollisionScore.CompareTo(B.CollisionScore); } public override Dictionary GetTopColliders() @@ -3889,7 +3440,7 @@ namespace OpenSim.Region.PhysicsModule.ODE lock (_prims) { List orderedPrims = new List(_prims); - orderedPrims.OrderByDescending(p => p.CollisionScore); + orderedPrims.Sort(compareByCollisionsDesc); topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore); foreach (OdePrim p in _prims) @@ -3940,133 +3491,6 @@ namespace OpenSim.Region.PhysicsModule.ODE return new List(ourResults); } -#if USE_DRAWSTUFF - // Keyboard callback - public void command(int cmd) - { - IntPtr geom; - d.Mass mass; - d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f); - - - - Char ch = Char.ToLower((Char)cmd); - switch ((Char)ch) - { - case 'w': - try - { - Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - - case 'a': - hpr.X++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - - case 's': - try - { - Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD)); - - xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z; - ds.SetViewpoint(ref xyz, ref hpr); - } - catch (ArgumentException) - { hpr.X = 0; } - break; - case 'd': - hpr.X--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'r': - xyz.Z++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'f': - xyz.Z--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'e': - xyz.Y++; - ds.SetViewpoint(ref xyz, ref hpr); - break; - case 'q': - xyz.Y--; - ds.SetViewpoint(ref xyz, ref hpr); - break; - } - } - - public void step(int pause) - { - - ds.SetColor(1.0f, 1.0f, 0.0f); - ds.SetTexture(ds.Texture.Wood); - lock (_prims) - { - foreach (OdePrim prm in _prims) - { - //IntPtr body = d.GeomGetBody(prm.prim_geom); - if (prm.prim_geom != IntPtr.Zero) - { - d.Vector3 pos; - d.GeomCopyPosition(prm.prim_geom, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(prm.prim_geom, out R); - //d.BodyCopyRotation(body, out R); - - - d.Vector3 sides = new d.Vector3(); - sides.X = prm.Size.X; - sides.Y = prm.Size.Y; - sides.Z = prm.Size.Z; - - ds.DrawBox(ref pos, ref R, ref sides); - } - } - } - ds.SetColor(1.0f, 0.0f, 0.0f); - - foreach (OdeCharacter chr in _characters) - { - if (chr.Shell != IntPtr.Zero) - { - IntPtr body = d.GeomGetBody(chr.Shell); - - d.Vector3 pos; - d.GeomCopyPosition(chr.Shell, out pos); - //d.BodyCopyPosition(body, out pos); - - d.Matrix3 R; - d.GeomCopyRotation(chr.Shell, out R); - //d.BodyCopyRotation(body, out R); - - ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f); - d.Vector3 sides = new d.Vector3(); - sides.X = 0.5f; - sides.Y = 0.5f; - sides.Z = 0.5f; - - ds.DrawBox(ref pos, ref R, ref sides); - } - } - } - - public void start(int unused) - { - ds.SetViewpoint(ref xyz, ref hpr); - } -#endif - public override Dictionary GetStats() { if (!CollectStats) diff --git a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs index 6dc22bd..2c134e7 100644 --- a/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs +++ b/OpenSim/Region/PhysicsModules/Ode/Tests/ODETestClass.cs @@ -47,6 +47,8 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests //private OpenSim.Region.PhysicsModule.ODE.OdePlugin cbt; private PhysicsScene pScene; + private OpenSim.Region.PhysicsModule.ODE.OdeModule odemodule; + [SetUp] public void Initialize() @@ -65,19 +67,18 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests info.RegionSizeX = info.RegionSizeY = info.RegionSizeZ = Constants.RegionSize; OpenSim.Region.Framework.Scenes.Scene scene = new OpenSim.Region.Framework.Scenes.Scene(info); - //IMesher mesher = new OpenSim.Region.PhysicsModules.Meshing.Meshmerizer(); + //IMesher mesher = new OpenSim.Region.PhysicsModule.Meshing.Meshmerizer(); //INonSharedRegionModule mod = mesher as INonSharedRegionModule; //mod.Initialise(openSimINI); //mod.AddRegion(scene); //mod.RegionLoaded(scene); - pScene = new OdeScene(); - Console.WriteLine("HERE " + (pScene == null ? "Null" : "Not null")); - INonSharedRegionModule mod = (pScene as INonSharedRegionModule); - Console.WriteLine("HERE " + (mod == null ? "Null" : "Not null")); - mod.Initialise(openSimINI); - mod.AddRegion(scene); - mod.RegionLoaded(scene); + // pScene = new OdeScene(); + odemodule = new OpenSim.Region.PhysicsModule.ODE.OdeModule(); + Console.WriteLine("HERE " + (odemodule == null ? "Null" : "Not null")); + odemodule.Initialise(openSimINI); + odemodule.AddRegion(scene); + odemodule.RegionLoaded(scene); // Loading ODEPlugin //cbt = new OdePlugin(); @@ -90,6 +91,7 @@ namespace OpenSim.Region.PhysicsModule.ODE.Tests { _heightmap[i] = 21f; } + pScene = scene.PhysicsScene; pScene.SetTerrain(_heightmap); } diff --git a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs b/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs deleted file mode 100644 index 87ca446..0000000 --- a/OpenSim/Region/PhysicsModules/Ode/drawstuff.cs +++ /dev/null @@ -1,98 +0,0 @@ -/* - * Copyright ODE - * 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. - * - * - */ - -using System; -using System.Runtime.InteropServices; -using Ode.NET; - -namespace Drawstuff.NET -{ -#if dDOUBLE - using dReal = System.Double; -#else - using dReal = System.Single; -#endif - - public static class ds - { - public const int VERSION = 2; - - public enum Texture - { - None, - Wood - } - - [UnmanagedFunctionPointer(CallingConvention.Cdecl)] - public delegate void CallbackFunction(int arg); - - [StructLayout(LayoutKind.Sequential)] - public struct Functions - { - public int version; - public CallbackFunction start; - public CallbackFunction step; - public CallbackFunction command; - public CallbackFunction stop; - public string path_to_textures; - } - - [DllImport("drawstuff", EntryPoint = "dsDrawBox")] - public static extern void DrawBox(ref d.Vector3 pos, ref d.Matrix3 R, ref d.Vector3 sides); - - [DllImport("drawstuff", EntryPoint = "dsDrawCapsule")] - public static extern void DrawCapsule(ref d.Vector3 pos, ref d.Matrix3 R, dReal length, dReal radius); - - [DllImport("drawstuff", EntryPoint = "dsDrawConvex")] - public static extern void DrawConvex(ref d.Vector3 pos, ref d.Matrix3 R, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); - - [DllImport("drawstuff", EntryPoint = "dsSetColor")] - public static extern void SetColor(float red, float green, float blue); - - [DllImport("drawstuff", EntryPoint = "dsSetTexture")] - public static extern void SetTexture(Texture texture); - - [DllImport("drawstuff", EntryPoint = "dsSetViewpoint")] - public static extern void SetViewpoint(ref d.Vector3 xyz, ref d.Vector3 hpr); - - [DllImport("drawstuff", EntryPoint = "dsSimulationLoop")] - public static extern void SimulationLoop(int argc, string[] argv, int window_width, int window_height, ref Functions fn); - } -} diff --git a/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs index e3a3e35..23fe9ae 100644 --- a/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/POS/AssemblyInfo.cs @@ -56,7 +56,7 @@ using Mono.Addins; // 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.8.2.*")] +[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] [assembly: Addin("OpenSim.Region.PhysicsModule.POS", OpenSim.VersionInfo.VersionNumber)] [assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs b/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs index 32469d9..341530e 100644 --- a/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs +++ b/OpenSim/Region/PhysicsModules/POS/POSCharacter.cs @@ -248,7 +248,7 @@ namespace OpenSim.Region.PhysicsModule.POS { } - public override void LockAngularMotion(Vector3 axis) + public override void LockAngularMotion(byte axislocks) { } @@ -273,10 +273,10 @@ namespace OpenSim.Region.PhysicsModule.POS set { return; } } - public override bool PIDActive - { + public override bool PIDActive + { get { return false; } - set { return; } + set { return; } } public override float PIDTau @@ -291,6 +291,7 @@ namespace OpenSim.Region.PhysicsModule.POS public override bool PIDHoverActive { + get { return false; } set { return; } } @@ -303,7 +304,7 @@ namespace OpenSim.Region.PhysicsModule.POS { set { return; } } - + public override Quaternion APIDTarget { set { return; } diff --git a/OpenSim/Region/PhysicsModules/POS/POSPrim.cs b/OpenSim/Region/PhysicsModules/POS/POSPrim.cs index 8aae716..180885d 100644 --- a/OpenSim/Region/PhysicsModules/POS/POSPrim.cs +++ b/OpenSim/Region/PhysicsModules/POS/POSPrim.cs @@ -250,7 +250,7 @@ namespace OpenSim.Region.PhysicsModule.POS { } - public override void LockAngularMotion(Vector3 axis) + public override void LockAngularMotion(byte axislocks) { } @@ -286,6 +286,7 @@ namespace OpenSim.Region.PhysicsModule.POS public override bool PIDHoverActive { + get { return false; } set { return; } } @@ -298,7 +299,7 @@ namespace OpenSim.Region.PhysicsModule.POS { set { return; } } - + public override Quaternion APIDTarget { set { return; } diff --git a/OpenSim/Region/PhysicsModules/POS/POSScene.cs b/OpenSim/Region/PhysicsModules/POS/POSScene.cs index 6375f18..e62d876 100644 --- a/OpenSim/Region/PhysicsModules/POS/POSScene.cs +++ b/OpenSim/Region/PhysicsModules/POS/POSScene.cs @@ -54,6 +54,11 @@ namespace OpenSim.Region.PhysicsModule.POS get { return "POS"; } } + public string Version + { + get { return "1.0"; } + } + public Type ReplaceableInterface { get { return null; } @@ -83,6 +88,7 @@ namespace OpenSim.Region.PhysicsModule.POS EngineType = Name; PhysicsSceneName = EngineType + "/" + scene.RegionInfo.RegionName; + EngineName = Name + " " + Version; scene.RegisterModuleInterface(this); base.Initialise(scene.PhysicsRequestAsset, @@ -288,7 +294,7 @@ namespace OpenSim.Region.PhysicsModule.POS character._velocity.Z = (character.Position.Z - oldposZ)/timeStep; } } - return fps; + return 1.0f; } public override void GetResults() diff --git a/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs index 33f60e4..9027e2e 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/AssemblyInfo.cs @@ -55,4 +55,4 @@ using System.Runtime.InteropServices; // 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.8.2.*")] +[assembly : AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs b/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs index 5c75307..1a8409e 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/IMesher.cs @@ -27,6 +27,7 @@ using System; using System.Collections.Generic; +using System.Runtime.InteropServices; using OpenSim.Framework; using OpenMetaverse; @@ -36,13 +37,18 @@ namespace OpenSim.Region.PhysicsModules.SharedBase { IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod); IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical); - IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde); + IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde); + IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex); + void ReleaseMesh(IMesh mesh); + void ExpireReleaseMeshs(); + void ExpireFileCache(); } // Values for level of detail to be passed to the mesher. // Values origionally chosen for the LOD of sculpties (the sqrt(width*heigth) of sculpt texture) // Lower level of detail reduces the number of vertices used to represent the meshed shape. - public enum LevelOfDetail + public enum LevelOfDetail { High = 32, Medium = 16, @@ -54,6 +60,25 @@ namespace OpenSim.Region.PhysicsModules.SharedBase { } + [Serializable()] + [StructLayout(LayoutKind.Explicit)] + public struct AMeshKey + { + [FieldOffset(0)] + public UUID uuid; + [FieldOffset(0)] + public ulong hashA; + [FieldOffset(8)] + public ulong hashB; + [FieldOffset(16)] + public ulong hashC; + + public override string ToString() + { + return uuid.ToString() + "-" + hashC.ToString("x") ; + } + } + public interface IMesh { List getVertexList(); @@ -67,5 +92,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase void releasePinned(); void Append(IMesh newMesh); void TransformLinear(float[,] matrix, float[] offset); + Vector3 GetCentroid(); + Vector3 GetOBB(); } } diff --git a/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs b/OpenSim/Region/PhysicsModules/SharedBase/IPhysicsParameters.cs old mode 100644 new mode 100755 diff --git a/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs index 432708c..8079e79 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/NullPhysicsScene.cs @@ -37,7 +37,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase class NullPhysicsScene : PhysicsScene { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - + private static int m_workIndicator; public override PhysicsActor AddAvatar( diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs index c04ff58..2fa98b5 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsActor.cs @@ -43,7 +43,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase Unknown = 0, Agent = 1, Prim = 2, - Ground = 3 + Ground = 3, + Water = 4 } public enum PIDHoverType @@ -54,20 +55,54 @@ namespace OpenSim.Region.PhysicsModules.SharedBase Absolute } + public struct CameraData + { + public Quaternion CameraRotation; + public Vector3 CameraAtAxis; + public bool MouseLook; + public bool Valid; + } + public struct ContactPoint { public Vector3 Position; public Vector3 SurfaceNormal; public float PenetrationDepth; + public float RelativeSpeed; + public bool CharacterFeet; public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth) { Position = position; SurfaceNormal = surfaceNormal; PenetrationDepth = penetrationDepth; + RelativeSpeed = 0f; // for now let this one be set explicity + CharacterFeet = true; // keep other plugins work as before + } + + public ContactPoint(Vector3 position, Vector3 surfaceNormal, float penetrationDepth, bool feet) + { + Position = position; + SurfaceNormal = surfaceNormal; + PenetrationDepth = penetrationDepth; + RelativeSpeed = 0f; // for now let this one be set explicity + CharacterFeet = feet; // keep other plugins work as before } } + public struct ContactData + { + public float mu; + public float bounce; + public bool softcolide; + + public ContactData(float _mu, float _bounce, bool _softcolide) + { + mu = _mu; + bounce = _bounce; + softcolide = _softcolide; + } + } /// /// Used to pass collision information to OnCollisionUpdate listeners. /// @@ -100,8 +135,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase } else { + float lastVel = m_objCollisionList[localID].RelativeSpeed; if (m_objCollisionList[localID].PenetrationDepth < contact.PenetrationDepth) + { + if(Math.Abs(lastVel) > Math.Abs(contact.RelativeSpeed)) + contact.RelativeSpeed = lastVel; m_objCollisionList[localID] = contact; + } + else if(Math.Abs(lastVel) < Math.Abs(contact.RelativeSpeed)) + { + ContactPoint tmp = m_objCollisionList[localID]; + tmp.RelativeSpeed = contact.RelativeSpeed; + m_objCollisionList[localID] = tmp; + } } } @@ -121,13 +167,15 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public delegate void RequestTerseUpdate(); public delegate void CollisionUpdate(EventArgs e); public delegate void OutOfBounds(Vector3 pos); + public delegate CameraData GetCameraData(); -// disable warning: public events + // disable warning: public events #pragma warning disable 67 public event PositionUpdate OnPositionUpdate; public event VelocityUpdate OnVelocityUpdate; public event OrientationUpdate OnOrientationUpdate; public event RequestTerseUpdate OnRequestTerseUpdate; + public event GetCameraData OnPhysicsRequestingCameraData; /// /// Subscribers to this event must synchronously handle the dictionary of collisions received, since the event @@ -138,15 +186,47 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public event OutOfBounds OnOutOfBounds; #pragma warning restore 67 + public CameraData TryGetCameraData() + { + GetCameraData handler = OnPhysicsRequestingCameraData; + if (handler != null) + { + return handler(); + } + + return new CameraData { Valid = false }; + } + public static PhysicsActor Null { get { return new NullPhysicsActor(); } } + public virtual bool Building { get; set; } + + public virtual void getContactData(ref ContactData cdata) + { + cdata.mu = 0; + cdata.bounce = 0; + } + public abstract bool Stopped { get; } public abstract Vector3 Size { get; set; } + public virtual void setAvatarSize(Vector3 size, float feetOffset) + { + Size = size; + } + + public virtual bool Phantom { get; set; } + + public virtual bool IsVolumeDtc + { + get { return false; } + set { return; } + } + public virtual byte PhysicsShapeType { get; set; } public abstract PrimitiveBaseShape Shape { set; } @@ -169,20 +249,21 @@ namespace OpenSim.Region.PhysicsModules.SharedBase /// XXX: Bizarrely, this cannot be "Terrain" or "Water" right now unless it really is simulating terrain or /// water. This is not a problem due to the formatting of names given by prims and avatars. /// - public string Name { get; protected set; } + public string Name { get; set; } /// /// This is being used by ODE joint code. /// public string SOPName; + public virtual void CrossingStart() { } public abstract void CrossingFailure(); public abstract void link(PhysicsActor obj); public abstract void delink(); - public abstract void LockAngularMotion(Vector3 axis); + public abstract void LockAngularMotion(byte axislocks); public virtual void RequestPhysicsterseUpdate() { @@ -245,6 +326,56 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract void VehicleRotationParam(int param, Quaternion rotation); public abstract void VehicleFlags(int param, bool remove); + // This is an overridable version of SetVehicle() that works for all physics engines. + // This is VERY inefficient. It behoves any physics engine to override this and + // implement a more efficient setting of all the vehicle parameters. + public virtual void SetVehicle(object pvdata) + { + VehicleData vdata = (VehicleData)pvdata; + // vehicleActor.ProcessSetVehicle((VehicleData)vdata); + + this.VehicleType = (int)vdata.m_type; + this.VehicleFlags(-1, false); // clears all flags + this.VehicleFlags((int)vdata.m_flags, false); + + // Linear properties + this.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_DIRECTION, vdata.m_linearMotorDirection); + this.VehicleVectorParam((int)Vehicle.LINEAR_FRICTION_TIMESCALE, vdata.m_linearFrictionTimescale); + this.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, vdata.m_linearMotorDecayTimescale); + this.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_TIMESCALE, vdata.m_linearMotorTimescale); + this.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_OFFSET, vdata.m_linearMotorOffset); + + //Angular properties + this.VehicleVectorParam((int)Vehicle.ANGULAR_MOTOR_DIRECTION, vdata.m_angularMotorDirection); + this.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_TIMESCALE, vdata.m_angularMotorTimescale); + this.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, vdata.m_angularMotorDecayTimescale); + this.VehicleVectorParam((int)Vehicle.ANGULAR_FRICTION_TIMESCALE, vdata.m_angularFrictionTimescale); + + //Deflection properties + this.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, vdata.m_angularDeflectionEfficiency); + this.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_TIMESCALE, vdata.m_angularDeflectionTimescale); + this.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_EFFICIENCY, vdata.m_linearDeflectionEfficiency); + this.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_TIMESCALE, vdata.m_linearDeflectionTimescale); + + //Banking properties + this.VehicleFloatParam((int)Vehicle.BANKING_EFFICIENCY, vdata.m_bankingEfficiency); + this.VehicleFloatParam((int)Vehicle.BANKING_MIX, vdata.m_bankingMix); + this.VehicleFloatParam((int)Vehicle.BANKING_TIMESCALE, vdata.m_bankingTimescale); + + //Hover and Buoyancy properties + this.VehicleFloatParam((int)Vehicle.HOVER_HEIGHT, vdata.m_VhoverHeight); + this.VehicleFloatParam((int)Vehicle.HOVER_EFFICIENCY, vdata.m_VhoverEfficiency); + this.VehicleFloatParam((int)Vehicle.HOVER_TIMESCALE, vdata.m_VhoverTimescale); + this.VehicleFloatParam((int)Vehicle.BUOYANCY, vdata.m_VehicleBuoyancy); + + //Attractor properties + this.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, vdata.m_verticalAttractionEfficiency); + this.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_TIMESCALE, vdata.m_verticalAttractionTimescale); + + this.VehicleRotationParam((int)Vehicle.REFERENCE_FRAME, vdata.m_referenceFrame); + } + + /// /// Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more /// @@ -253,6 +384,22 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract Vector3 GeometricCenter { get; } public abstract Vector3 CenterOfMass { get; } + public virtual float PhysicsCost + { + get + { + return 0.1f; + } + } + + public virtual float StreamCost + { + get + { + return 1.0f; + } + } + /// /// The desired velocity of this actor. /// @@ -271,6 +418,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase } public abstract Vector3 Velocity { get; set; } + public virtual Vector3 rootVelocity { get { return Vector3.Zero; } } public abstract Vector3 Torque { get; set; } public abstract float CollisionScore { get; set;} @@ -296,7 +444,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase // Used for llSetHoverHeight and maybe vehicle height // Hover Height will override MoveTo target's Z - public abstract bool PIDHoverActive { set;} + public abstract bool PIDHoverActive {get; set;} public abstract float PIDHoverHeight { set;} public abstract PIDHoverType PIDHoverType { set;} public abstract float PIDHoverTau { set;} @@ -306,7 +454,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract bool APIDActive { set;} public abstract float APIDStrength { set;} public abstract float APIDDamping { set;} - + public abstract void AddForce(Vector3 force, bool pushforce); public abstract void AddAngularForce(Vector3 force, bool pushforce); public abstract void SetMomentum(Vector3 momentum); @@ -314,6 +462,29 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract void UnSubscribeEvents(); public abstract bool SubscribedEvents(); + public virtual void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { } + public virtual void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact) { } + + public virtual PhysicsInertiaData GetInertiaData() + { + PhysicsInertiaData data = new PhysicsInertiaData(); + data.TotalMass = this.Mass; + data.CenterOfMass = CenterOfMass - Position; + data.Inertia = Vector3.Zero; + data.InertiaRotation = Vector4.Zero; + return data; + } + + public virtual void SetInertiaData(PhysicsInertiaData inertia) + { + } + + public virtual float SimulationSuspended { get; set; } + + // Warning in a parent part it returns itself, not null + public virtual PhysicsActor ParentActor { get { return this; } } + + // Extendable interface for new, physics engine specific operations public virtual object Extension(string pFunct, params object[] pParams) { @@ -324,9 +495,11 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public class NullPhysicsActor : PhysicsActor { + private ActorTypes m_actorType = ActorTypes.Unknown; + public override bool Stopped { - get{ return false; } + get{ return true; } } public override Vector3 Position @@ -343,6 +516,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public override uint LocalID { + get { return 0; } set { return; } } @@ -402,50 +576,17 @@ namespace OpenSim.Region.PhysicsModules.SharedBase set { return; } } - public override void VehicleFloatParam(int param, float value) - { + 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 void SetMaterial(int material) {} + public override Vector3 CenterOfMass { get { return Vector3.Zero; }} - } - - public override void VehicleVectorParam(int param, Vector3 value) - { + public override Vector3 GeometricCenter { get { return Vector3.Zero; }} - } - - public override void VehicleRotationParam(int param, Quaternion rotation) - { - - } - - public override void VehicleFlags(int param, bool remove) - { - - } - - public override void SetVolumeDetect(int param) - { - - } - - public override void SetMaterial(int material) - { - - } - - public override Vector3 CenterOfMass - { - get { return Vector3.Zero; } - } - - public override Vector3 GeometricCenter - { - get { return Vector3.Zero; } - } - - public override PrimitiveBaseShape Shape - { - set { return; } - } + public override PrimitiveBaseShape Shape { set { return; }} public override Vector3 Velocity { @@ -465,9 +606,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase set { } } - public override void CrossingFailure() - { - } + public override void CrossingFailure() {} public override Quaternion Orientation { @@ -507,8 +646,20 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public override int PhysicsActorType { - get { return (int) ActorTypes.Unknown; } - set { return; } + get { return (int)m_actorType; } + set { + ActorTypes type = (ActorTypes)value; + switch (type) + { + case ActorTypes.Ground: + case ActorTypes.Water: + m_actorType = type; + break; + default: + m_actorType = ActorTypes.Unknown; + break; + } + } } public override bool Kinematic @@ -517,26 +668,11 @@ namespace OpenSim.Region.PhysicsModules.SharedBase set { return; } } - public override void link(PhysicsActor obj) - { - } - - public override void delink() - { - } - - public override void LockAngularMotion(Vector3 axis) - { - } - - public override void AddForce(Vector3 force, bool pushforce) - { - } - - public override void AddAngularForce(Vector3 force, bool pushforce) - { - - } + public override void link(PhysicsActor obj) { } + public override void delink() { } + public override void LockAngularMotion(byte axislocks) { } + public override void AddForce(Vector3 force, bool pushforce) { } + public override void AddAngularForce(Vector3 force, bool pushforce) { } public override Vector3 RotationalVelocity { @@ -546,39 +682,28 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public override Vector3 PIDTarget { set { return; } } - public override bool PIDActive + public override bool PIDActive { get { return false; } - set { return; } + set { return; } } public override float PIDTau { set { return; } } public override float PIDHoverHeight { set { return; } } - public override bool PIDHoverActive { set { return; } } + public override bool PIDHoverActive {get {return false;} 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 SetMomentum(Vector3 momentum) - { - } - - public override void SubscribeEvents(int ms) - { - } - public override void UnSubscribeEvents() - { + public override void SetMomentum(Vector3 momentum) { } - } - public override bool SubscribedEvents() - { - return false; - } + public override void SubscribeEvents(int ms) { } + public override void UnSubscribeEvents() { } + public override bool SubscribedEvents() { return false; } } } diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs index 32691fc..e8d6334 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsScene.cs @@ -41,6 +41,10 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal); public delegate void RayCallback(List list); + public delegate void ProbeBoxCallback(List list); + public delegate void ProbeSphereCallback(List list); + public delegate void ProbePlaneCallback(List list); + public delegate void SitAvatarCallback(int status, uint partID, Vector3 offset, Quaternion Orientation); public delegate void JointMoved(PhysicsJoint joint); public delegate void JointDeactivated(PhysicsJoint joint); @@ -89,6 +93,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public Vector3 Normal; } + + public abstract class PhysicsScene { // private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); @@ -106,6 +112,8 @@ namespace OpenSim.Region.PhysicsModules.SharedBase /// public string EngineType { get; protected set; } + public string EngineName { get; protected set; } + // The only thing that should register for this event is the SceneGraph // Anything else could cause problems. public event physicsCrash OnPhysicsCrash; @@ -143,6 +151,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase /// /// /// + public abstract PhysicsActor AddAvatar( string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying); @@ -161,12 +170,26 @@ namespace OpenSim.Region.PhysicsModules.SharedBase { PhysicsActor ret = AddAvatar(avName, position, velocity, size, isFlying); - if (ret != null) + if (ret != null) ret.LocalID = localID; return ret; } + public virtual PhysicsActor AddAvatar( + uint localID, string avName, Vector3 position, Vector3 size, bool isFlying) + { + PhysicsActor ret = AddAvatar(localID, avName, position, Vector3.Zero, size, isFlying); + return ret; + } + + public virtual PhysicsActor AddAvatar( + uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) + { + PhysicsActor ret = AddAvatar(localID, avName, position, Vector3.Zero, size, isFlying); + return ret; + } + /// /// Remove an avatar. /// @@ -182,6 +205,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase 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 PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + return AddPrimShape(primName, pbs, position, size, rotation, isPhysical, localid); + } + + public virtual PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapetype, uint localid) { @@ -255,6 +291,9 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public abstract void AddPhysicsActorTaint(PhysicsActor prim); + + public virtual void ProcessPreSimulation() { } + /// /// Perform a simulation of the current physics scene over the given timestep. /// @@ -293,28 +332,19 @@ namespace OpenSim.Region.PhysicsModules.SharedBase return false; } - public virtual bool SupportsCombining() - { - return false; - } - - public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} - - public virtual void UnCombine(PhysicsScene pScene) {} - /// /// Queue a raycast against the physics scene. /// The provided callback method will be called when the raycast is complete - /// - /// Many physics engines don't support collision testing at the same time as - /// manipulating the physics scene, so we queue the request up and callback + /// + /// Many physics engines don't support collision testing at the same time as + /// manipulating the physics scene, so we queue the request up and callback /// a custom method when the raycast is complete. /// This allows physics engines that give an immediate result to callback immediately /// and ones that don't, to callback when it gets a result back. - /// + /// /// ODE for example will not allow you to change the scene while collision testing or /// it asserts, 'opteration not valid for locked space'. This includes adding a ray to the scene. - /// + /// /// This is named RayCastWorld to not conflict with modrex's Raycast method. /// /// Origin of the ray @@ -348,6 +378,31 @@ namespace OpenSim.Region.PhysicsModules.SharedBase return false; } + public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + return new List(); + } + + public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) + { + return 0; + } + // Extendable interface for new, physics engine specific operations public virtual object Extension(string pFunct, params object[] pParams) { diff --git a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs index 76a82fa..5aae42f 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/PhysicsVector.cs @@ -49,7 +49,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase public Vector3(Vector3 pv) : this(pv.X, pv.Y, pv.Z) { } - + public void setValues(float x, float y, float z) { X = x; diff --git a/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs b/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs index 63a8cb8..0b4c8c6 100644 --- a/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs +++ b/OpenSim/Region/PhysicsModules/SharedBase/VehicleConstants.cs @@ -26,6 +26,7 @@ */ using System; +using OpenMetaverse; namespace OpenSim.Region.PhysicsModules.SharedBase { @@ -37,7 +38,7 @@ namespace OpenSim.Region.PhysicsModules.SharedBase TYPE_NONE = 0, /// - /// No Angular motor, High Left right friction, No Hover, Linear Deflection 1, no angular deflection + /// No Angular motor, High Left right friction, No Hover, Linear Deflection 1, no angular deflection /// no vertical attractor, No banking, Identity rotation frame /// TYPE_SLED = 1, @@ -117,5 +118,47 @@ namespace OpenSim.Region.PhysicsModules.SharedBase 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/PhysicsModules/ubOde/ODEApi.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs new file mode 100644 index 0000000..35adf11 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEApi.cs @@ -0,0 +1,2024 @@ +/* + * 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/ + * changes by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; +using OMV = OpenMetaverse; +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; + + public const uint CONTACTS_UNIMPORTANT = 0x80000000; + + #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, + ubtTerrainClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 5 + } + + 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 dReal OSTerrainGetHeight(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 = "dCreateOSTerrain"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateiOSTerrain(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateOSTerrain(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiOSTerrain(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 uint 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 uint 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)); } + } + public static OMV.Vector3 GeomGetPositionOMV(IntPtr geom) + { + Vector3 vtmp = GeomGetPosition(geom); + return new OMV.Vector3(vtmp.X, vtmp.Y, vtmp.Z); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + public static OMV.Quaternion GeomGetQuaternionOMV(IntPtr geom) + { + Quaternion qtmp; + GeomCopyQuaternion(geom, out qtmp); + return new OMV.Quaternion(qtmp.X, qtmp.Y, qtmp.Z, qtmp.W); + } + + [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 = "dGeomUbitTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataBuild(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataBuild"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataBuild(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal sampleSize, int widthSamples, int depthSamples, + dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomOSTerrainDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomOSTerrainGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomOSTerrainSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomOSTerrainSetHeightfieldData(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, uint bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, uint 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 IntPtr iGetConfiguration(); + + public static string GetConfiguration() + { + IntPtr ptr = iGetConfiguration(); + string s = Marshal.PtrToStringAnsi(ptr); + return s; + } + + [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/PhysicsModules/ubOde/ODECharacter.cs b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs new file mode 100644 index 0000000..0e46471 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODECharacter.cs @@ -0,0 +1,2036 @@ +/* + * 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.PhysicsModules.SharedBase; +using log4net; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// 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 Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private Vector3 m_size; + private Vector3 m_collideNormal; + private Vector3 m_lastFallVel; + private Quaternion m_orientation; + private Quaternion m_orientation2D; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + + const float basePID_D = 0.55f; // scaled for unit mass unit time (2200 /(50*80)) + const float basePID_P = 0.225f; // scaled for unit mass unit time (900 /(50*80)) + public float PID_D; + public float PID_P; + + private float timeStep; + private float invtimeStep; + + private float m_feetOffset = 0; + private float feetOff = 0; + private float boneOff = 0; + private float AvaAvaSizeXsq = 0.3f; + private float AvaAvaSizeYsq = 0.2f; + + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool m_flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + + private bool _zeroFlag = false; + private bool m_haveLastFallVel = false; + + private 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 bool m_freemove = false; + +// 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.Character + | CollisionCategories.Geom + | CollisionCategories.VolumeDtc + ); + // we do land collisions not ode | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private ODEScene m_parent_scene; + private IntPtr capsule = IntPtr.Zero; + public IntPtr collider = IntPtr.Zero; + + public IntPtr Amotor = IntPtr.Zero; + + public d.Mass ShellMass; + + public int m_eventsubscription = 0; + private int m_cureventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + private bool SentEmptyCollisionsEvent; + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + float mu; + + // HoverHeight control + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType; + private float m_targetHoverHeight; + + + public OdeCharacter(uint localID, String avName, ODEScene parent_scene, Vector3 pos, Vector3 pSize, float pfeetOffset, float density, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + m_localID = localID; + m_parent_scene = parent_scene; + + timeStep = parent_scene.ODE_STEPSIZE; + invtimeStep = 1 / timeStep; + + 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)m_parent_scene.WorldExtents.X * 0.5f), ((float)m_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + m_size.X = pSize.X; + m_size.Y = pSize.Y; + m_size.Z = pSize.Z; + + if(m_size.X <0.01f) + m_size.X = 0.01f; + if(m_size.Y <0.01f) + m_size.Y = 0.01f; + if(m_size.Z <0.01f) + m_size.Z = 0.01f; + + m_feetOffset = pfeetOffset; + m_orientation = Quaternion.Identity; + m_orientation2D = Quaternion.Identity; + m_density = density; + + // force lower density for testing + m_density = 3.0f; + + mu = m_parent_scene.AvatarFriction; + + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; ; // sure we have a default + + PID_D = basePID_D * m_mass * invtimeStep; + PID_P = basePID_P * m_mass * invtimeStep; + + m_isPhysical = false; // current status: no ODE information exists + + Name = avName; + + AddChange(changes.Add, null); + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = 0; + cdata.softcolide = false; + } + + 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 + { + get { return m_localID; } + set { m_localID = value; } + } + + public override PhysicsActor ParentActor + { + get { return (PhysicsActor)this; } + } + + 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 m_isPhysical; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return m_flying; } + set + { + m_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 += 3; + if (m_colliderfilter > 3) + m_colliderfilter = 3; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + { + m_pidControllerActive = true; + m_iscolliding = true; + m_freemove = false; + } + } + } + + /// + /// 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 (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = m_parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -100f) + { + value.Z = m_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 + { + return m_size; + } + set + { + if (value.IsFinite()) + { + if(value.X <0.01f) + value.X = 0.01f; + if(value.Y <0.01f) + value.Y = 0.01f; + if(value.Z <0.01f) + value.Z = 0.01f; + + AddChange(changes.Size, value); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + public override void setAvatarSize(Vector3 size, float feetOffset) + { + if (size.IsFinite()) + { + if (size.X < 0.01f) + size.X = 0.01f; + if (size.Y < 0.01f) + size.Y = 0.01f; + if (size.Z < 0.01f) + size.Z = 0.01f; + + strAvatarSize st = new strAvatarSize(); + st.size = size; + st.offset = feetOffset; + AddChange(changes.AvatarSize, st); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN AvatarSize 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 + { + return m_mass; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(byte axislocks) + { + + } + + + 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; + } + } + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 rootVelocity + { + get + { + return _velocity; + } + } + + 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 TargetVelocity + { + get + { + return m_targetVelocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.TargetVelocity, 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 m_orientation; } + set + { +// fakeori = value; +// givefakeori++; + value.Normalize(); + AddChange(changes.Orientation, value); + } + } + + 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 / (m_parent_scene.ODE_STEPSIZE * 28f)); + } + else + { + AddChange(changes.TargetVelocity, 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) + { + if (momentum.IsFinite()) + AddChange(changes.Momentum, momentum); + } + + + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + // sizes one day should came from visual parameters + float sx = m_size.X; + float sy = m_size.Y; + float sz = m_size.Z; + + float bot = -sz * 0.5f + m_feetOffset; + boneOff = bot + 0.3f; + + float feetsz = sz * 0.45f; + if (feetsz > 0.6f) + feetsz = 0.6f; + + feetOff = bot + feetsz; + + AvaAvaSizeXsq = 0.4f * sx; + AvaAvaSizeXsq *= AvaAvaSizeXsq; + AvaAvaSizeYsq = 0.5f * sy; + AvaAvaSizeYsq *= AvaAvaSizeYsq; + + m_parent_scene.waitForSpaceUnlock(m_parent_scene.CharsSpace); + + collider = d.HashSpaceCreate(m_parent_scene.CharsSpace); + d.HashSpaceSetLevels(collider, -4, 3); + d.SpaceSetSublevel(collider, 3); + d.SpaceSetCleanup(collider, false); + d.GeomSetCategoryBits(collider, (uint)m_collisionCategories); + d.GeomSetCollideBits(collider, (uint)m_collisionFlags); + + float r = m_size.X; + if (m_size.Y > r) + r = m_size.Y; + float l = m_size.Z - r; + r *= 0.5f; + + capsule = d.CreateCapsule(collider, r, l); + + m_mass = m_density * m_size.X * m_size.Y * m_size.Z; // update mass + + d.MassSetBoxTotal(out ShellMass, m_mass, m_size.X, m_size.Y, m_size.Z); + + PID_D = basePID_D * m_mass / m_parent_scene.ODE_STEPSIZE; + PID_P = basePID_P * m_mass / m_parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(m_parent_scene.world); + + _zeroFlag = false; + m_pidControllerActive = true; + m_freemove = false; + + _velocity = Vector3.Zero; + + 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(capsule, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(m_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, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e8f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e8f); + } + + /// + /// 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 Geoms + if (capsule != IntPtr.Zero) + { + m_parent_scene.actor_name_map.Remove(capsule); + m_parent_scene.waitForSpaceUnlock(collider); + d.GeomDestroy(capsule); + capsule = IntPtr.Zero; + } + + if (collider != IntPtr.Zero) + { + d.SpaceDestroy(collider); + collider = IntPtr.Zero; + } + + } + + //in place 2D rotation around Z assuming rot is normalised and is a rotation around Z + public void RotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void RotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + public void invRotateXYonZ(ref float x, ref float y, ref float sin, ref float cos) + { + float tx = x; + x = tx * cos + y * sin; + y = -tx * sin + y * cos; + } + + public void invRotateXYonZ(ref float x, ref float y, ref Quaternion rot) + { + float sin = - 2.0f * rot.Z * rot.W; + float cos = rot.W * rot.W - rot.Z * rot.Z; + float tx = x; + + x = tx * cos - y * sin; + y = tx * sin + y * cos; + } + + public bool Collide(IntPtr me, IntPtr other, bool reverse, ref d.ContactGeom contact, + ref d.ContactGeom altContact , ref bool useAltcontact, ref bool feetcollision) + { + feetcollision = false; + useAltcontact = false; + + if (me == capsule) + { + Vector3 offset; + + float h = contact.pos.Z - _position.Z; + offset.Z = h - feetOff; + + offset.X = contact.pos.X - _position.X; + offset.Y = contact.pos.Y - _position.Y; + + d.GeomClassID gtype = d.GeomGetClass(other); + if (gtype == d.GeomClassID.CapsuleClass) + { + Vector3 roff = offset * Quaternion.Inverse(m_orientation2D); + float r = roff.X *roff.X / AvaAvaSizeXsq; + r += (roff.Y * roff.Y) / AvaAvaSizeYsq; + if (r > 1.0f) + return false; + + float dp = 1.0f -(float)Math.Sqrt((double)r); + if (dp > 0.05f) + dp = 0.05f; + + contact.depth = dp; + + if (offset.Z < 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + } + return true; + } + + if (gtype == d.GeomClassID.SphereClass && d.GeomGetBody(other) != IntPtr.Zero) + { + if(d.GeomSphereGetRadius(other) < 0.5) + return true; + } + + if (offset.Z > 0 || contact.normal.Z > 0.35f) + { + if (offset.Z <= 0) + { + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + } + return true; + } + + if(m_flying) + return true; + + feetcollision = true; + if (h < boneOff) + { + m_collideNormal.X = contact.normal.X; + m_collideNormal.Y = contact.normal.Y; + m_collideNormal.Z = contact.normal.Z; + IsColliding = true; + } + + altContact = contact; + useAltcontact = true; + + offset.Z -= 0.2f; + + offset.Normalize(); + + float tdp = contact.depth; + float t = offset.X; + t = Math.Abs(t); + if(t > 1e-6) + { + tdp /= t; + tdp *= contact.normal.X; + } + else + tdp *= 10; + + if (tdp > 0.25f) + tdp = 0.25f; + + altContact.depth = tdp; + + if (reverse) + { + altContact.normal.X = offset.X; + altContact.normal.Y = offset.Y; + altContact.normal.Z = offset.Z; + } + else + { + altContact.normal.X = -offset.X; + altContact.normal.Y = -offset.Y; + altContact.normal.Z = -offset.Z; + } + return true; + } + return false; + } + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(List defects) + { + if (Body == IntPtr.Zero) + return; + + d.Vector3 dtmp = d.BodyGetPosition(Body); + 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 = m_orientation2D.W; + qtmp.X = m_orientation2D.X; + qtmp.Y = m_orientation2D.Y; + qtmp.Z = m_orientation2D.Z; + d.BodySetQuaternion(Body, ref qtmp); + + if (m_pidControllerActive == false) + { + _zeroPosition = localpos; + } + + 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; + } + + // check outbounds forcing to be in world + bool fixbody = false; + if (localpos.X < 0.0f) + { + fixbody = true; + localpos.X = 0.1f; + } + else if (localpos.X > m_parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + localpos.X = m_parent_scene.WorldExtents.X - 0.1f; + } + if (localpos.Y < 0.0f) + { + fixbody = true; + localpos.Y = 0.1f; + } + else if (localpos.Y > m_parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + localpos.Y = m_parent_scene.WorldExtents.Y - 0.1f; + } + if (fixbody) + { + m_freemove = false; + d.BodySetPosition(Body, localpos.X, localpos.Y, localpos.Z); + } + + float breakfactor; + + Vector3 vec = Vector3.Zero; + dtmp = d.BodyGetLinearVel(Body); + Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + float velLengthSquared = vel.LengthSquared(); + + Vector3 ctz = _target_velocity; + + float movementdivisor = 1f; + //Ubit change divisions into multiplications below + if (!m_alwaysRun) + movementdivisor = 1 / walkDivisor; + else + movementdivisor = 1 / runDivisor; + + ctz.X *= movementdivisor; + ctz.Y *= movementdivisor; + + //****************************************** + // colide with land + + d.AABB aabb; +// d.GeomGetAABB(feetbox, out aabb); + d.GeomGetAABB(capsule, out aabb); + float chrminZ = aabb.MinZ; // move up a bit + Vector3 posch = localpos; + + float ftmp; + + if (m_flying) + { + ftmp = timeStep; + posch.X += vel.X * ftmp; + posch.Y += vel.Y * ftmp; + } + + float terrainheight = m_parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + if (chrminZ < terrainheight) + { + if (ctz.Z < 0) + ctz.Z = 0; + + if(!m_haveLastFallVel) + { + m_lastFallVel = vel; + m_haveLastFallVel = true; + } + + Vector3 n = m_parent_scene.GetTerrainNormalAtXY(posch.X, posch.Y); + float depth = terrainheight - chrminZ; + + vec.Z = depth * PID_P * 50; + + if (!m_flying) + { + vec.Z += -vel.Z * PID_D; + if(n.Z < 0.4f) + { + vec.X = depth * PID_P * 50 - vel.X * PID_D; + vec.X *= n.X; + vec.Y = depth * PID_P * 50 - vel.Y * PID_D; + vec.Y *= n.Y; + vec.Z *= n.Z; + if(n.Z < 0.1f) + { + // cancel the slope pose + n.X = 0f; + n.Y = 0f; + n.Z = 1.0f; + } + } + } + + if (depth < 0.2f) + { + m_colliderGroundfilter++; + if (m_colliderGroundfilter > 2) + { + m_iscolliding = true; + m_colliderfilter = 2; + + if (m_colliderGroundfilter > 10) + { + m_colliderGroundfilter = 10; + m_freemove = false; + } + + m_collideNormal.X = n.X; + m_collideNormal.Y = n.Y; + m_collideNormal.Z = n.Z; + + m_iscollidingGround = true; + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = terrainheight; + contact.SurfaceNormal.X = -n.X; + contact.SurfaceNormal.Y = -n.Y; + contact.SurfaceNormal.Z = -n.Z; + contact.RelativeSpeed = Vector3.Dot(m_lastFallVel, n); + contact.CharacterFeet = true; + AddCollisionEvent(0, contact); + m_lastFallVel = vel; + +// vec.Z *= 0.5f; + } + } + + else + { + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } + } + } + else + { + m_haveLastFallVel = false; + m_colliderGroundfilter -= 5; + if (m_colliderGroundfilter <= 0) + { + m_colliderGroundfilter = 0; + m_iscollidingGround = false; + } + } + + bool hoverPIDActive = false; + + if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { + hoverPIDActive = true; + + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = terrainheight + m_PIDHoverHeight; + break; + + case PIDHoverType.GroundAndWater: + float waterHeight = m_parent_scene.GetWaterLevel(); + if (terrainheight > waterHeight) + m_targetHoverHeight = terrainheight + m_PIDHoverHeight; + else + m_targetHoverHeight = waterHeight + m_PIDHoverHeight; + break; + } // end switch (m_PIDHoverType) + + // don't go underground + if (m_targetHoverHeight > terrainheight + 0.5f * (aabb.MaxZ - aabb.MinZ)) + { + float fz = (m_targetHoverHeight - localpos.Z); + + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) + { + ctz.Z = 0; + } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; + + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); + + ctz.Z = fz; + } + } + } + + //****************************************** + if (!m_iscolliding) + m_collideNormal.Z = 0; + + bool tviszero = (ctz.X == 0.0f && ctz.Y == 0.0f && ctz.Z == 0.0f); + + if (!tviszero) + { + m_freemove = false; + + // movement relative to surface if moving on it + // dont disturbe vertical movement, ie jumps + if (m_iscolliding && !m_flying && ctz.Z == 0 && m_collideNormal.Z > 0.2f && m_collideNormal.Z < 0.94f) + { + float p = ctz.X * m_collideNormal.X + ctz.Y * m_collideNormal.Y; + ctz.X *= (float)Math.Sqrt(1 - m_collideNormal.X * m_collideNormal.X); + ctz.Y *= (float)Math.Sqrt(1 - m_collideNormal.Y * m_collideNormal.Y); + ctz.Z -= p; + if (ctz.Z < 0) + ctz.Z *= 2; + + } + + } + + if (!m_freemove) + { + + // if velocity is zero, use position control; otherwise, velocity control + if (tviszero && m_iscolliding && !m_flying) + { + // 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 * 2f + (_zeroPosition.X - localpos.X) * (PID_P * 5); + vec.Y = -vel.Y * PID_D * 2f + (_zeroPosition.Y - localpos.Y) * (PID_P * 5); + if(vel.Z > 0) + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + else + vec.Z += (-vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P) * 0.2f; +/* + 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 (!m_flying) + { + // we are on a surface + if (ctz.Z > 0f) + { + // moving up or JUMPING + vec.Z += (ctz.Z - vel.Z) * PID_D * 2f; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + else + { + // we are moving down on a surface + if (ctz.Z == 0) + { + if (vel.Z > 0) + vec.Z -= vel.Z * PID_D * 2f; + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + // intencionally going down + else + { + if (ctz.Z < vel.Z) + vec.Z += (ctz.Z - vel.Z) * PID_D; + else + { + } + + if (Math.Abs(ctz.X) > Math.Abs(vel.X)) + vec.X += (ctz.X - vel.X) * (PID_D); + if (Math.Abs(ctz.Y) > Math.Abs(vel.Y)) + vec.Y += (ctz.Y - vel.Y) * (PID_D); + } + } + + // We're standing on something + } + else + { + // We're flying and colliding with something + vec.X += (ctz.X - vel.X) * (PID_D * 0.0625f); + vec.Y += (ctz.Y - vel.Y) * (PID_D * 0.0625f); + vec.Z += (ctz.Z - vel.Z) * (PID_D * 0.0625f); + } + } + else // ie not colliding + { + if (m_flying || hoverPIDActive) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X += (ctz.X - vel.X) * (PID_D); + vec.Y += (ctz.Y - vel.Y) * (PID_D); + vec.Z += (ctz.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 += (ctz.X - vel.X) * PID_D * 0.833f; + vec.Y += (ctz.Y - vel.Y) * PID_D * 0.833f; + // hack for breaking on fall + if (ctz.Z == -9999f) + vec.Z += -vel.Z * PID_D - m_parent_scene.gravityz * m_mass; + } + } + } + + if (velLengthSquared > 2500.0f) // 50m/s apply breaks + { + breakfactor = 0.16f * m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + vec.Z -= breakfactor * vel.Z; + } + } + else + { + breakfactor = m_mass; + vec.X -= breakfactor * vel.X; + vec.Y -= breakfactor * vel.Y; + if (m_flying) + vec.Z -= 0.5f * breakfactor * vel.Z; + else + vec.Z -= .16f* m_mass * vel.Z; + } + + if (m_flying || hoverPIDActive) + { + vec.Z -= m_parent_scene.gravityz * m_mass; + + if(!hoverPIDActive) + { + //Added for auto fly height. Kitto Flora + float target_altitude = terrainheight + 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(); + return; + } + + // update our local ideia of position velocity and aceleration + // _position = localpos; + _position = localpos; + + if (_zeroFlag) + { + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } + else + { + Vector3 a =_velocity; // previus velocity + SetSmooth(ref _velocity, ref vel, 2); + a = (_velocity - a) * invtimeStep; + SetSmooth(ref _acceleration, ref a, 2); + + dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = 0f; + m_rotationalVelocity.Y = 0f; + m_rotationalVelocity.Z = dtmp.Z; + Math.Round(m_rotationalVelocity.Z,3); + } + } + + public void round(ref Vector3 v, int digits) + { + v.X = (float)Math.Round(v.X, digits); + v.Y = (float)Math.Round(v.Y, digits); + v.Z = (float)Math.Round(v.Z, digits); + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value) + { + dst.X = 0.1f * dst.X + 0.9f * value.X; + dst.Y = 0.1f * dst.Y + 0.9f * value.Y; + dst.Z = 0.1f * dst.Z + 0.9f * value.Z; + } + + public void SetSmooth(ref Vector3 dst, ref Vector3 value, int rounddigits) + { + dst.X = 0.4f * dst.X + 0.6f * value.X; + dst.X = (float)Math.Round(dst.X, rounddigits); + + dst.Y = 0.4f * dst.Y + 0.6f * value.Y; + dst.Y = (float)Math.Round(dst.Y, rounddigits); + + dst.Z = 0.4f * dst.Z + 0.6f * value.Z; + dst.Z = (float)Math.Round(dst.Z, rounddigits); + } + + + /// + /// Updates the reported position and velocity. + /// Used to copy variables from unmanaged space at heartbeat rate and also trigger scene updates acording + /// also outbounds checking + /// copy and outbounds now done in move(..) at ode rate + /// + /// + public void UpdatePositionAndVelocity() + { + return; + +// if (Body == IntPtr.Zero) +// return; + + } + + /// + /// 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 {get {return m_pidControllerActive;} set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight + { + set + { + AddChange(changes.PIDHoverHeight,value); + } + } + public override bool PIDHoverActive + { + get + { + return m_useHoverPID; + } + set + { + AddChange(changes.PIDHoverActive, value); + } + } + + public override PIDHoverType PIDHoverType + { + set + { + AddChange(changes.PIDHoverType,value); + } + } + + public override float PIDHoverTau + { + set + { + float tmp =0; + if (value > 0) + { + float mint = (0.05f > timeStep ? 0.05f : timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDHoverTau, tmp); + } + } + + 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_eventsubscription = ms; + m_cureventsubscription = 0; + CollisionEventsThisFrame.Clear(); + SentEmptyCollisionsEvent = false; + } + + public override void UnSubscribeEvents() + { + m_eventsubscription = 0; + m_parent_scene.RemoveCollisionEventReporting(this); + lock(CollisionEventsThisFrame) + CollisionEventsThisFrame.Clear(); + } + + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + lock(CollisionEventsThisFrame) + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + m_parent_scene.AddCollisionEventReporting(this); + } + + public void SendCollisions(int timestep) + { + if (m_cureventsubscription < 50000) + m_cureventsubscription += timestep; + + if (m_cureventsubscription < m_eventsubscription) + return; + + lock(CollisionEventsThisFrame) + { + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + m_cureventsubscription = 0; + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; + // _parent_scene.RemoveCollisionEventReporting(this); + } + else + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + private void changePhysicsStatus(bool NewStatus) + { + if (NewStatus != m_isPhysical) + { + if (NewStatus) + { + AvatarGeomAndBodyDestroy(); + + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); + + m_parent_scene.actor_name_map[collider] = (PhysicsActor)this; + m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this; + m_parent_scene.AddCharacter(this); + } + else + { + m_parent_scene.RemoveCollisionEventReporting(this); + m_parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + m_freemove = false; + m_isPhysical = NewStatus; + } + } + + private void changeAdd() + { + changePhysicsStatus(true); + } + + private void changeRemove() + { + changePhysicsStatus(false); + } + + private void changeShape(PrimitiveBaseShape arg) + { + } + + private void changeAvatarSize(strAvatarSize st) + { + m_feetOffset = st.offset; + changeSize(st.size); + } + + private void changeSize(Vector3 pSize) + { + if (pSize.IsFinite()) + { + // for now only look to Z changes since viewers also don't change X and Y + if (pSize.Z != m_size.Z) + { + AvatarGeomAndBodyDestroy(); + + float oldsz = m_size.Z; + m_size = pSize; + + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (m_size.Z - oldsz) * 0.5f); + +// Velocity = Vector3.Zero; + m_targetVelocity = Vector3.Zero; + + m_parent_scene.actor_name_map[collider] = (PhysicsActor)this; + m_parent_scene.actor_name_map[capsule] = (PhysicsActor)this; + } + m_freemove = false; + 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; + m_freemove = false; + m_pidControllerActive = true; + } + + private void changeOrientation(Quaternion newOri) + { + if (m_orientation != newOri) + { + m_orientation = newOri; // keep a copy for core use + // but only use rotations around Z + + m_orientation2D.W = newOri.W; + m_orientation2D.Z = newOri.Z; + + float t = m_orientation2D.W * m_orientation2D.W + m_orientation2D.Z * m_orientation2D.Z; + if (t > 0) + { + t = 1.0f / (float)Math.Sqrt(t); + m_orientation2D.W *= t; + m_orientation2D.Z *= t; + } + else + { + m_orientation2D.W = 1.0f; + m_orientation2D.Z = 0f; + } + m_orientation2D.Y = 0f; + m_orientation2D.X = 0f; + + d.Quaternion myrot = new d.Quaternion(); + myrot.X = m_orientation2D.X; + myrot.Y = m_orientation2D.Y; + myrot.Z = m_orientation2D.Z; + myrot.W = m_orientation2D.W; + d.BodySetQuaternion(Body, ref myrot); + } + } + + private void changeVelocity(Vector3 newVel) + { + _velocity = newVel; + setFreeMove(); + + if (Body != IntPtr.Zero) + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + + private void changeTargetVelocity(Vector3 newVel) + { + m_pidControllerActive = true; + m_freemove = false; + _target_velocity = newVel; + } + + private void changeSetTorque(Vector3 newTorque) + { + } + + private void changeAddForce(Vector3 newForce) + { + } + + private void changeAddAngularForce(Vector3 arg) + { + } + + private void changeAngularLock(byte 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 setFreeMove() + { + m_pidControllerActive = true; + _zeroFlag = false; + _target_velocity = Vector3.Zero; + m_freemove = true; + m_colliderfilter = -1; + m_colliderObjectfilter = -1; + m_colliderGroundfilter = -1; + + m_iscolliding = false; + m_iscollidingGround = false; + m_iscollidingObj = false; + + CollisionEventsThisFrame.Clear(); + } + + private void changeForce(Vector3 newForce) + { + setFreeMove(); + + if (Body != IntPtr.Zero) + { + if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) + d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); + } + } + + // for now momentum is actually velocity + private void changeMomentum(Vector3 newmomentum) + { + _velocity = newmomentum; + setFreeMove(); + + if (Body != IntPtr.Zero) + d.BodySetLinearVel(Body, newmomentum.X, newmomentum.Y, newmomentum.Z); + } + + private void changePIDHoverHeight(float val) + { + m_PIDHoverHeight = val; + if (val == 0) + m_useHoverPID = false; + } + + private void changePIDHoverType(PIDHoverType type) + { + m_PIDHoverType = type; + } + + private void changePIDHoverTau(float tau) + { + m_PIDHoverTau = tau; + } + + private void changePIDHoverActive(bool active) + { + m_useHoverPID = active; + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (collider == 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.TargetVelocity: + changeTargetVelocity((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((byte)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.AvatarSize: + changeAvatarSize((strAvatarSize)arg); + break; + + case changes.Momentum: + changeMomentum((Vector3)arg); + break; + + case changes.PIDHoverHeight: + changePIDHoverHeight((float)arg); + break; + + case changes.PIDHoverType: + changePIDHoverType((PIDHoverType)arg); + break; + + case changes.PIDHoverTau: + changePIDHoverTau((float)arg); + break; + + case changes.PIDHoverActive: + changePIDHoverActive((bool)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) + { + m_parent_scene.AddChange((PhysicsActor)this, what, arg); + } + + private struct strAvatarSize + { + public Vector3 size; + public float offset; + } + + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs new file mode 100644 index 0000000..ce10065 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEDynamics.cs @@ -0,0 +1,1205 @@ +/* + * 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. + */ + +// Extensive change 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.PhysicsModules.SharedBase; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + private OdePrim rootPrim; + private ODEScene _pParentScene; + + // Vehicle properties + // WARNING this are working copies for internel use + // their values may not be the corresponding parameter + + 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_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 + + //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 = 1000; + + //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 float m_lmEfect = 0f; // current linear motor eficiency + private float m_lmDecay = 0f; // current linear decay + + private float m_amEfect = 0; // current angular motor eficiency + private float m_amDecay = 0f; // current linear decay + + private float m_ffactor = 1.0f; + + private float m_timestep = 0.02f; + private float m_invtimestep = 50; + + + float m_ampwr; + float m_amdampX; + float m_amdampY; + float m_amdampZ; + + float m_gravmod; + + public float FrictionFactor + { + get + { + return m_ffactor; + } + } + + public float GravMod + { + set + { + m_gravmod = value; + } + } + + + public ODEDynamics(OdePrim rootp) + { + rootPrim = rootp; + _pParentScene = rootPrim._parent_scene; + m_timestep = _pParentScene.ODE_STEPSIZE; + m_invtimestep = 1.0f / m_timestep; + m_gravmod = rootPrim.GravModifier; + } + + public void DoSetVehicle(VehicleData vd) + { + m_type = vd.m_type; + m_flags = vd.m_flags; + + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + if (m_linearFrictionTimescale.X < m_timestep) m_linearFrictionTimescale.X = m_timestep; + if (m_linearFrictionTimescale.Y < m_timestep) m_linearFrictionTimescale.Y = m_timestep; + if (m_linearFrictionTimescale.Z < m_timestep) m_linearFrictionTimescale.Z = m_timestep; + + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + if (m_linearMotorDecayTimescale < m_timestep) m_linearMotorDecayTimescale = m_timestep; + m_linearMotorDecayTimescale += 0.2f; + m_linearMotorDecayTimescale *= m_invtimestep; + + m_linearMotorTimescale = vd.m_linearMotorTimescale; + if (m_linearMotorTimescale < m_timestep) m_linearMotorTimescale = m_timestep; + + m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + if (m_angularMotorTimescale < m_timestep) m_angularMotorTimescale = m_timestep; + + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + if (m_angularMotorDecayTimescale < m_timestep) m_angularMotorDecayTimescale = m_timestep; + m_angularMotorDecayTimescale *= m_invtimestep; + + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + if (m_angularFrictionTimescale.X < m_timestep) m_angularFrictionTimescale.X = m_timestep; + if (m_angularFrictionTimescale.Y < m_timestep) m_angularFrictionTimescale.Y = m_timestep; + if (m_angularFrictionTimescale.Z < m_timestep) m_angularFrictionTimescale.Z = m_timestep; + + //Deflection properties + m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; + m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; + if (m_angularDeflectionTimescale < m_timestep) m_angularDeflectionTimescale = m_timestep; + + m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; + m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + if (m_linearDeflectionTimescale < m_timestep) m_linearDeflectionTimescale = m_timestep; + + //Banking properties + m_bankingEfficiency = vd.m_bankingEfficiency; + m_bankingMix = vd.m_bankingMix; + m_bankingTimescale = vd.m_bankingTimescale; + if (m_bankingTimescale < m_timestep) m_bankingTimescale = m_timestep; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; + m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + if (m_VhoverTimescale < m_timestep) m_VhoverTimescale = m_timestep; + + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + if (m_verticalAttractionTimescale < m_timestep) m_verticalAttractionTimescale = m_timestep; + + // Axis + m_referenceFrame = vd.m_referenceFrame; + + m_lmEfect = 0; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_amEfect = 0; + m_ffactor = 1.0f; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + if(float.IsNaN(pValue) || float.IsInfinity(pValue)) + return; + + 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 < m_timestep) pValue = m_timestep; + m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + else if (pValue > 120) pValue = 120; + m_angularMotorDecayTimescale = pValue * m_invtimestep; + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < m_timestep) pValue = m_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 < m_timestep) pValue = m_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 < m_timestep) pValue = m_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 < m_timestep) pValue = m_timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < m_timestep) pValue = m_timestep; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = (0.2f +pValue) * m_invtimestep; + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < m_timestep) pValue = m_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 < m_timestep) pValue = m_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 < m_timestep) pValue = m_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 + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < m_timestep) pValue = m_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 > 100.0f) + m_linearMotorDirection *= (100.0f / len); + + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + m_lmEfect = 1.0f; // turn it on + + m_ffactor = 0.0f; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + 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; + if(!pValue.IsFinite()) + return; + + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_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 + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < m_timestep) pValue.X = m_timestep; + if (pValue.Y < m_timestep) pValue.Y = m_timestep; + if (pValue.Z < m_timestep) pValue.Z = m_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 > 100.0f) + m_linearMotorDirection *= (100.0f / len); + + m_lmEfect = 1.0f; // turn it on + m_lmDecay = 1.0f - 1.0f / m_linearMotorDecayTimescale; + + m_ffactor = 0.0f; + if (rootPrim.Body != IntPtr.Zero && !d.BodyIsEnabled(rootPrim.Body) + && !rootPrim.m_isSelected && !rootPrim.m_disabled) + d.BodyEnable(rootPrim.Body); + 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); + m_referenceFrame = 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) + { + m_lmEfect = 0; + + m_amEfect = 0; + m_ffactor = 1f; + + 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 * m_invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 1000; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 1000; + m_verticalAttractionEfficiency = 0; + m_verticalAttractionTimescale = 1000; + + 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 * m_invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120 * m_invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 1; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 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 * m_invtimestep; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f * m_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 * m_invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4 * m_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_UP_ONLY | // new sl + 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 * m_invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8 * m_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 * m_invtimestep; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10 * m_invtimestep; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 5 * m_invtimestep; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + 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 | //); + VehicleFlag.LIMIT_ROLL_ONLY | // new sl + VehicleFlag.HOVER_GLOBAL_HEIGHT); // new sl + +// m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | +// VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + // disable mouse steering + m_flags &= ~(VehicleFlag.MOUSELOOK_STEER | + VehicleFlag.MOUSELOOK_BANK | + VehicleFlag.CAMERA_DECOUPLED); + + m_lmDecay = (1.0f - 1.0f / m_linearMotorDecayTimescale); + m_amDecay = 1.0f - 1.0f / m_angularMotorDecayTimescale; + + }//end SetDefaultsForType + + internal void Stop() + { + m_lmEfect = 0; + m_lmDecay = 0f; + m_amEfect = 0; + m_amDecay = 0; + m_ffactor = 1f; + } + + 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 pi = (float)Math.PI; + private const float halfpi = 0.5f * (float)Math.PI; + private const float twopi = 2.0f * pi; + + public static Vector3 ubRot2Euler(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() + { + IntPtr Body = rootPrim.Body; + + d.Mass dmass; + d.BodyGetMass(Body, out dmass); + + 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 + + float ldampZ = 0; + + bool mousemode = false; + bool mousemodebank = false; + + float bankingEfficiency; + float verticalAttractionTimescale = m_verticalAttractionTimescale; + + if((m_flags & (VehicleFlag.MOUSELOOK_STEER | VehicleFlag.MOUSELOOK_BANK)) != 0 ) + { + mousemode = true; + mousemodebank = (m_flags & VehicleFlag.MOUSELOOK_BANK) != 0; + if(mousemodebank) + { + bankingEfficiency = m_bankingEfficiency; + if(verticalAttractionTimescale < 149.9) + verticalAttractionTimescale *= 2.0f; // reduce current instability + } + else + bankingEfficiency = 0; + } + else + bankingEfficiency = m_bankingEfficiency; + + // 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 *= dmass.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 *= m_lmDecay; +// m_ffactor = 0.01f + 1e-4f * curVel.LengthSquared(); + m_ffactor = 0.0f; + } + else + { + m_lmEfect = 0; + m_ffactor = 1f; + } + + // hover + if (m_VhoverTimescale < 300 && rootPrim.prim_geom != IntPtr.Zero) + { + // d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 pos = d.GeomGetPosition(rootPrim.prim_geom); + pos.Z -= 0.21f; // minor offset that seems to be always there in sl + + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float perr; + + // default to global but don't go underground + perr = m_VhoverHeight - pos.Z; + + if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += t; + } + else + { + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + } + else if (t > m_VhoverHeight) + perr = t - pos.Z; ; + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > -0.1) + { + ldampZ = m_VhoverEfficiency * m_invtimestep; + + perr *= (1.0f + ldampZ) / m_VhoverTimescale; + + // force.Z += perr - curVel.Z * tmp; + force.Z += perr; + ldampZ *= -curVel.Z; + + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; + } + else + { + // default gravity and Buoyancy + force.Z += _pParentScene.gravityz * m_gravmod * (1f - m_VehicleBuoyancy); + } + + // linear deflection + if (m_linearDeflectionEfficiency > 0) + { + float len = curVel.Length(); + if (len > 0.01) // if moving + { + 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; + } + } + } + + // linear friction/damping + 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 + + if(ldampZ != 0 && Math.Abs(ldampZ) > Math.Abs(tmpV.Z)) + tmpV.Z = ldampZ; + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + // vertical atractor + if (verticalAttractionTimescale < 300) + { + float roll; + float pitch; + + float ftmp = m_invtimestep / verticalAttractionTimescale / verticalAttractionTimescale; + + float ftmp2; + ftmp2 = 0.5f * m_verticalAttractionEfficiency * m_invtimestep; + m_amdampX = ftmp2; + + m_ampwr = 1.0f - 0.8f * m_verticalAttractionEfficiency; + + GetRollPitch(irotq, out roll, out pitch); + + if (roll > halfpi) + roll = pi - roll; + else if (roll < -halfpi) + roll = -pi - roll; + + float effroll = pitch / halfpi; + effroll *= effroll; + effroll = 1 - effroll; + effroll *= roll; + + torque.X += effroll * ftmp; + + if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0) + { + float effpitch = roll / halfpi; + effpitch *= effpitch; + effpitch = 1 - effpitch; + effpitch *= pitch; + + torque.Y += effpitch * ftmp; + } + + if (bankingEfficiency != 0 && Math.Abs(effroll) > 0.01) + { + + float broll = effroll; + /* + if (broll > halfpi) + broll = pi - broll; + else if (broll < -halfpi) + broll = -pi - broll; + */ + broll *= 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 + (vfact - 1) * m_bankingMix); + else + broll *= -(1 + (vfact - 1) * m_bankingMix); + } + // make z rot be in world Z not local as seems to be in sl + + broll = broll / m_bankingTimescale; + + + tmpV = Zrot(irotq); + tmpV *= broll; + + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; + + m_amdampZ = Math.Abs(m_bankingEfficiency) / m_bankingTimescale; + m_amdampY = m_amdampZ; + + } + else + { + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + m_amdampY = m_amdampX; + } + } + else + { + m_ampwr = 1.0f; + m_amdampX = 1 / m_angularFrictionTimescale.X; + m_amdampY = 1 / m_angularFrictionTimescale.Y; + m_amdampZ = 1 / m_angularFrictionTimescale.Z; + } + + if(mousemode) + { + CameraData cam = rootPrim.TryGetCameraData(); + if(cam.Valid && cam.MouseLook) + { + Vector3 dirv = cam.CameraAtAxis * irotq; + + float invamts = 1.0f/m_angularMotorTimescale; + float tmp; + + // get out of x == 0 plane + if(Math.Abs(dirv.X) < 0.001f) + dirv.X = 0.001f; + + if (Math.Abs(dirv.Z) > 0.01) + { + tmp = -(float)Math.Atan2(dirv.Z, dirv.X) * m_angularMotorDirection.Y; + if(tmp < -4f) + tmp = -4f; + else if(tmp > 4f) + tmp = 4f; + torque.Y += (tmp - curLocalAngVel.Y) * invamts; + torque.Y -= curLocalAngVel.Y * m_amdampY; + } + else + torque.Y -= curLocalAngVel.Y * m_invtimestep; + + if (Math.Abs(dirv.Y) > 0.01) + { + if(mousemodebank) + { + tmp = -(float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.X; + if(tmp < -4f) + tmp = -4f; + else if(tmp > 4f) + tmp = 4f; + torque.X += (tmp - curLocalAngVel.X) * invamts; + } + else + { + tmp = (float)Math.Atan2(dirv.Y, dirv.X) * m_angularMotorDirection.Z; + tmp *= invamts; + if(tmp < -4f) + tmp = -4f; + else if(tmp > 4f) + tmp = 4f; + torque.Z += (tmp - curLocalAngVel.Z) * invamts; + } + torque.X -= curLocalAngVel.X * m_amdampX; + torque.Z -= curLocalAngVel.Z * m_amdampZ; + } + else + { + if(mousemodebank) + torque.X -= curLocalAngVel.X * m_invtimestep; + else + torque.Z -= curLocalAngVel.Z * m_invtimestep; + } + } + else + { + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + torque.X -= curLocalAngVel.X * 10f; + torque.Y -= curLocalAngVel.Y * 10f; + torque.Z -= curLocalAngVel.Z * 10f; + } + } + } + else + { + // 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 * m_ampwr; + torque.Y += tmpV.Y * m_ampwr; + torque.Z += tmpV.Z; + + m_amEfect *= m_amDecay; + } + else + m_amEfect = 0; + + // 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; + } + } + + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + torque.X -= curLocalAngVel.X * m_amdampX; + torque.Y -= curLocalAngVel.Y * m_amdampY; + torque.Z -= curLocalAngVel.Z * m_amdampZ; + } + } + + force *= dmass.mass; + + force += rootPrim.m_force; + force += rootPrim.m_forceacc; + rootPrim.m_forceacc = Vector3.Zero; + + if (force.X != 0 || force.Y != 0 || force.Z != 0) + { + d.BodyAddForce(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 + } + + torque = rootPrim.m_torque; + torque += rootPrim.m_angularForceacc; + rootPrim.m_angularForceacc = Vector3.Zero; + if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) + d.BodyAddTorque(Body,torque.X, torque.Y, torque.Z); + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs new file mode 100644 index 0000000..5465035 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEMeshWorker.cs @@ -0,0 +1,946 @@ +/* + * AJLDuarte 2012 + */ + +using System; +using System.Threading; +using System.Collections.Generic; +using System.IO; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using Nini.Config; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public enum MeshState : byte + { + noNeed = 0, + + loadingAsset = 1, + + AssetOK = 0x0f, // 00001111 + + NeedMask = 0x30, // 00110000 + needMesh = 0x10, // 00010000 + needAsset = 0x20, // 00100000 + + FailMask = 0xC0, // 11000000 + AssetFailed = 0x40, // 01000000 + MeshFailed = 0x80, // 10000000 + + MeshNoColide = FailMask | needAsset + } + + public enum meshWorkerCmnds : byte + { + nop = 0, + addnew, + changefull, + changesize, + changeshapetype, + getmesh, + } + + public class ODEPhysRepData + { + public PhysicsActor actor; + public PrimitiveBaseShape pbs; + public IMesh mesh; + + public Vector3 size; + public Vector3 OBB; + public Vector3 OBBOffset; + + public float volume; + + public byte shapetype; + public bool hasOBB; + public bool hasMeshVolume; + public bool isTooSmall; + public MeshState meshState; + public UUID? assetID; + public meshWorkerCmnds comand; + } + + public class ODEMeshWorker + { + private ILog m_log; + private ODEScene m_scene; + private IMesher m_mesher; + + public bool meshSculptedPrim = true; + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 32; + public float MinSizeToMeshmerize = 0.1f; + + private OpenSim.Framework.BlockingQueue workQueue = new OpenSim.Framework.BlockingQueue(); + private bool m_running; + + private Thread m_thread; + + public ODEMeshWorker(ODEScene pScene, ILog pLog, IMesher pMesher, IConfig pConfig) + { + m_scene = pScene; + m_log = pLog; + m_mesher = pMesher; + + if (pConfig != null) + { + meshSculptedPrim = pConfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim); + meshSculptLOD = pConfig.GetFloat("mesh_lod", meshSculptLOD); + MinSizeToMeshmerize = pConfig.GetFloat("mesh_min_size", MinSizeToMeshmerize); + MeshSculptphysicalLOD = pConfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD); + } + m_running = true; + m_thread = new Thread(DoWork); + m_thread.Name = "OdeMeshWorker"; + m_thread.Start(); + } + + private void DoWork() + { + m_mesher.ExpireFileCache(); + + while(m_running) + { + ODEPhysRepData nextRep = workQueue.Dequeue(); + if(!m_running) + return; + if (nextRep == null) + continue; + if (m_scene.haveActor(nextRep.actor)) + { + switch (nextRep.comand) + { + case meshWorkerCmnds.changefull: + case meshWorkerCmnds.changeshapetype: + case meshWorkerCmnds.changesize: + GetMesh(nextRep); + if (CreateActorPhysRep(nextRep) && m_scene.haveActor(nextRep.actor)) + m_scene.AddChange(nextRep.actor, changes.PhysRepData, nextRep); + break; + case meshWorkerCmnds.getmesh: + DoRepDataGetMesh(nextRep); + break; + } + } + } + } + + public void Stop() + { + try + { + m_thread.Abort(); + workQueue.Clear(); + } + catch + { + } + } + + public void ChangeActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + CheckMesh(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.PhysRepData, repData); + return; + } + + public ODEPhysRepData NewActorPhysRep(PhysicsActor actor, PrimitiveBaseShape pbs, + Vector3 size, byte shapetype) + { + ODEPhysRepData repData = new ODEPhysRepData(); + repData.actor = actor; + repData.pbs = pbs; + repData.size = size; + repData.shapetype = shapetype; + + CheckMesh(repData); + CalcVolumeData(repData); + m_scene.AddChange(actor, changes.AddPhysRep, repData); + return repData; + } + + public void RequestMesh(ODEPhysRepData repData) + { + repData.mesh = null; + + if (repData.meshState == MeshState.needAsset) + { + PrimitiveBaseShape pbs = repData.pbs; + + // check if we got outdated + + if (!pbs.SculptEntry || pbs.SculptTexture == UUID.Zero) + { + repData.meshState = MeshState.noNeed; + return; + } + + repData.assetID = pbs.SculptTexture; + repData.meshState = MeshState.loadingAsset; + + repData.comand = meshWorkerCmnds.getmesh; + workQueue.Enqueue(repData); + } + } + + // creates and prepares a mesh to use and calls parameters estimation + public bool CreateActorPhysRep(ODEPhysRepData repData) + { + IMesh mesh = repData.mesh; + + if (mesh != null) + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on prim {0} mesh UUID {1}", + repData.actor.Name, repData.pbs.SculptTexture.ToString()); + repData.meshState = MeshState.MeshFailed; + repData.hasOBB = false; + repData.mesh = null; + m_scene.mesher.ReleaseMesh(mesh); + } + else + { + repData.OBBOffset = mesh.GetCentroid(); + repData.OBB = mesh.GetOBB(); + repData.hasOBB = true; + mesh.releaseSourceMeshData(); + } + } + CalcVolumeData(repData); + return true; + } + + public void AssetLoaded(ODEPhysRepData repData) + { + if (m_scene.haveActor(repData.actor)) + { + if (needsMeshing(repData)) // no need for pbs now? + { + repData.comand = meshWorkerCmnds.changefull; + workQueue.Enqueue(repData); + } + } + else + repData.pbs.SculptData = Utils.EmptyBytes; + } + + public void DoRepDataGetMesh(ODEPhysRepData repData) + { + if (!repData.pbs.SculptEntry) + return; + + if (repData.meshState != MeshState.loadingAsset) + return; + + if (repData.assetID == null || repData.assetID == UUID.Zero) + return; + + if (repData.assetID != repData.pbs.SculptTexture) + return; + + // check if it is in cache + GetMesh(repData); + if (repData.meshState != MeshState.needAsset) + { + CreateActorPhysRep(repData); + m_scene.AddChange(repData.actor, changes.PhysRepData, repData); + return; + } + + RequestAssetDelegate assetProvider = m_scene.RequestAssetMethod; + if (assetProvider == null) + return; + ODEAssetRequest asr = new ODEAssetRequest(this, assetProvider, repData, m_log); + } + + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(ODEPhysRepData repData) + { + PrimitiveBaseShape pbs = repData.pbs; + // check sculpts or meshs + + Vector3 scale = pbs.Scale; + if(scale.X <= MinSizeToMeshmerize && + scale.Y <= MinSizeToMeshmerize && + scale.Z <= MinSizeToMeshmerize) + { + repData.isTooSmall = true; + return false; + } + + if (pbs.SculptEntry) + { + if (meshSculptedPrim) + return true; + + if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs + return true; + + return false; + } + + // convex shapes have no holes + ushort profilehollow = pbs.ProfileHollow; + if(repData.shapetype == 2) + profilehollow = 0; + + // 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 ((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 + && 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) + { + 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 + + int iPropertiesNotSupportedDefault = 0; + + if (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 (iPropertiesNotSupportedDefault == 0) + { + return false; + } + return true; + } + + // see if we need a mesh and if so if we have a cached one + // called with a new repData + public void CheckMesh(ODEPhysRepData repData) + { + PhysicsActor actor = repData.actor; + PrimitiveBaseShape pbs = repData.pbs; + + if (!needsMeshing(repData)) + { + repData.meshState = MeshState.noNeed; + repData.hasOBB = false; + return; + } + + IMesh mesh = null; + + Vector3 size = repData.size; + + int clod = (int)LevelOfDetail.High; + byte shapetype = repData.shapetype; + bool convex = shapetype == 2; + + mesh = m_mesher.GetMesh(actor.Name, pbs, size, clod, true, convex); + + if (mesh == null) + { + if (pbs.SculptEntry) + { + if (pbs.SculptTexture != null && pbs.SculptTexture != UUID.Zero) + { + repData.assetID = pbs.SculptTexture; + repData.meshState = MeshState.needAsset; + } + else + repData.meshState = MeshState.MeshFailed; + + return; + } + else + { + repData.meshState = MeshState.needMesh; + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + if (mesh == null) + { + repData.meshState = MeshState.MeshFailed; + return; + } + } + } + + repData.meshState = MeshState.AssetOK; + repData.mesh = mesh; + repData.OBB = mesh.GetOBB(); + repData.OBBOffset = mesh.GetCentroid(); + repData.hasOBB = true; + + if (pbs.SculptEntry) + { + repData.assetID = pbs.SculptTexture; + } + + pbs.SculptData = Utils.EmptyBytes; + return ; + } + + public void GetMesh(ODEPhysRepData repData) + { + PhysicsActor actor = repData.actor; + + PrimitiveBaseShape pbs = repData.pbs; + + repData.mesh = null; + repData.hasOBB = false; + + if (!needsMeshing(repData)) + { + repData.meshState = MeshState.noNeed; + return; + } + + if (repData.meshState == MeshState.MeshFailed) + return; + + if (pbs.SculptEntry) + { + if (repData.meshState == MeshState.AssetFailed) + { + if (pbs.SculptTexture == repData.assetID) + return; + } + } + + repData.meshState = MeshState.noNeed; + + IMesh mesh = null; + Vector3 size = repData.size; + byte shapetype = repData.shapetype; + + bool convex; + int clod = (int)LevelOfDetail.High; + if (shapetype == 0) + convex = false; + else + { + convex = true; + if (pbs.SculptType != (byte)SculptType.Mesh) + clod = (int)LevelOfDetail.Low; + } + + mesh = m_mesher.CreateMesh(actor.Name, pbs, size, clod, true, convex, true); + + if (mesh == null) + { + if (pbs.SculptEntry) + { + if (pbs.SculptTexture == UUID.Zero) + return; + + repData.assetID = pbs.SculptTexture; + + if (pbs.SculptData == null || pbs.SculptData.Length == 0) + { + repData.meshState = MeshState.needAsset; + return; + } + } + } + + repData.mesh = mesh; + repData.pbs.SculptData = Utils.EmptyBytes; + + if (mesh == null) + { + if (pbs.SculptEntry) + repData.meshState = MeshState.AssetFailed; + else + repData.meshState = MeshState.MeshFailed; + + return; + } + + repData.meshState = MeshState.AssetOK; + + return; + } + + private void CalculateBasicPrimVolume(ODEPhysRepData repData) + { + Vector3 _size = repData.size; + + float volume = _size.X * _size.Y * _size.Z; // default + if(repData.isTooSmall) + { + repData.volume = volume; + return; + } + + PrimitiveBaseShape _pbs = repData.pbs; + 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.5236f; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Circle: + case HollowShape.Triangle: // diference in sl is minor and odd + case HollowShape.Same: + break; + + case HollowShape.Square: + hollowVolume *= 0.909f; + break; + + // case HollowShape.Triangle: + // hollowVolume *= .827f; + // break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + + } + 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); + + repData.volume = volume; + } + + private void CalcVolumeData(ODEPhysRepData repData) + { + if (repData.hasOBB) + { + Vector3 OBB = repData.OBB; + } + else + { + Vector3 OBB = repData.size; + OBB.X *= 0.5f; + OBB.Y *= 0.5f; + OBB.Z *= 0.5f; + + repData.OBB = OBB; + repData.OBBOffset = Vector3.Zero; + } + + CalculateBasicPrimVolume(repData); + } + } + + public class ODEAssetRequest + { + ODEMeshWorker m_worker; + private ILog m_log; + ODEPhysRepData repData; + + public ODEAssetRequest(ODEMeshWorker pWorker, RequestAssetDelegate provider, + ODEPhysRepData pRepData, ILog plog) + { + m_worker = pWorker; + m_log = plog; + repData = pRepData; + + repData.meshState = MeshState.AssetFailed; + if (provider == null) + return; + + if (repData.assetID == null) + return; + + UUID assetID = (UUID) repData.assetID; + if (assetID == UUID.Zero) + return; + + repData.meshState = MeshState.loadingAsset; + provider(assetID, ODEassetReceived); + } + + void ODEassetReceived(AssetBase asset) + { + repData.meshState = MeshState.AssetFailed; + if (asset != null) + { + if (asset.Data != null && asset.Data.Length > 0) + { + repData.meshState = MeshState.noNeed; + + if (!repData.pbs.SculptEntry) + return; + if (repData.pbs.SculptTexture != repData.assetID) + return; + +// repData.pbs.SculptData = new byte[asset.Data.Length]; +// asset.Data.CopyTo(repData.pbs.SculptData,0); + repData.pbs.SculptData = asset.Data; + repData.meshState = MeshState.AssetOK; + m_worker.AssetLoaded(repData); + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned invalid mesh data for prim {0} asset UUID {1}.", + repData.actor.Name, asset.ID.ToString()); + } + else + m_log.WarnFormat("[PHYSICS]: asset provider returned null asset for mesh of prim {0}.", + repData.actor.Name); + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs new file mode 100644 index 0000000..4cb1736 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEModule.cs @@ -0,0 +1,112 @@ +using System; +using System.Collections.Generic; +using System.Reflection; +using log4net; +using Nini.Config; +using Mono.Addins; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEPhysicsScene")] + class ubOdeModule : INonSharedRegionModule + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private static Dictionary m_scenes = new Dictionary(); + private bool m_Enabled = false; + private IConfigSource m_config; + private bool OSOdeLib; + + + #region INonSharedRegionModule + + public string Name + { + get { return "ubODE"; } + } + + public string Version + { + get { return "1.0"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void Initialise(IConfigSource source) + { + IConfig config = source.Configs["Startup"]; + if (config != null) + { + string physics = config.GetString("physics", string.Empty); + if (physics == Name) + { + m_config = source; + m_Enabled = true; + + if (Util.IsWindows()) + Util.LoadArchSpecificWindowsDll("ode.dll"); + + d.InitODE(); + + string ode_config = d.GetConfiguration(); + if (ode_config != null && ode_config != "") + { + m_log.InfoFormat("[ubODE] ode library configuration: {0}", ode_config); + + if (ode_config.Contains("ODE_OPENSIM")) + { + OSOdeLib = true; + } + } + } + } + } + + public void Close() + { + } + + public void AddRegion(Scene scene) + { + if (!m_Enabled) + return; + + if(m_scenes.ContainsKey(scene)) // ??? + return; + ODEScene newodescene = new ODEScene(scene, m_config, Name, Version, OSOdeLib); + m_scenes[scene] = newodescene; + } + + public void RemoveRegion(Scene scene) + { + if (!m_Enabled) + return; + + // a odescene.dispose is called later directly by scene.cs + // since it is seen as a module interface + + if(m_scenes.ContainsKey(scene)) + m_scenes.Remove(scene); + } + + public void RegionLoaded(Scene scene) + { + if (!m_Enabled) + return; + + if(m_scenes.ContainsKey(scene)) + { + m_scenes[scene].RegionLoaded(); + } + + } + #endregion + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs new file mode 100644 index 0000000..aa208e2 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEPrim.cs @@ -0,0 +1,4256 @@ +/* + * 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 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.PhysicsModules.SharedBase; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + private bool m_isphantom; + private bool m_fakeisphantom; + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + internal bool m_building; + protected bool m_forcePosOrRotation; + private bool m_iscolliding; + + internal bool m_isSelected; + private bool m_delaySelect; + private bool m_lastdoneSelected; + internal bool m_outbounds; + + private byte m_angularlocks = 0; + + private Quaternion m_lastorientation; + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 m_acceleration; + private IntPtr Amotor; + + internal Vector3 m_force; + internal Vector3 m_forceacc; + internal Vector3 m_torque; + internal Vector3 m_angularForceacc; + + private float m_invTimeStep; + private float m_timeStep; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private bool m_usePID; + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType; + 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 m_body_autodisable_frames; + public int m_bodydisablecontrol = 0; + private float m_gravmod = 1.0f; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + // Default colide nonphysical don't try to colide with anything + private const CollisionCategories m_default_collisionFlagsNotPhysical = 0; + + private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Land | + CollisionCategories.VolumeDtc); + +// private bool m_collidesLand = true; + private bool m_collidesWater; +// public bool m_returnCollisions; + + private bool m_NoColide; // for now only for internal use for bad meshs + + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical; + + public bool m_disabled; + + private uint m_localID; + + private IMesh m_mesh; + private object m_meshlock = new object(); + private PrimitiveBaseShape _pbs; + + private UUID? m_assetID; + private MeshState m_meshState; + + public ODEScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + public float m_collisionscore; + private int m_colliderfilter = 0; + + public IntPtr collide_geom; // for objects: geom if single prim space it linkset + + private float m_density; + private byte m_shapetype; + private byte m_fakeShapetype; + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body; + + private Vector3 _target_velocity; + + public Vector3 m_OBBOffset; + public Vector3 m_OBB; + public float primOOBradiusSQ; + + private bool m_hasOBB = true; + + private float m_physCost; + private float m_streamCost; + + public d.Mass primdMass; // prim inertia information on it's own referencial + private PhysicsInertiaData m_InertiaOverride; + float primMass; // prim own mass + float primVolume; // prim own volume; + float m_mass; // object mass acording to case + + public int givefakepos; + private Vector3 fakepos; + public int givefakeori; + private Quaternion fakeori; + private PhysicsInertiaData m_fakeInertiaOverride; + + private int m_eventsubscription; + private int m_cureventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = null; + private CollisionEventUpdate CollisionVDTCEventsThisFrame = null; + private bool SentEmptyCollisionsEvent; + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + private float mu; + private float bounce; + + /// + /// 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 IsVolumeDtc + { + get { return m_fakeisVolumeDetect; } + set + { + m_fakeisVolumeDetect = value; + AddChange(changes.VolumeDtc, value); + } + } + + public override bool Phantom // this is not reliable for internal use + { + get { return m_fakeisphantom; } + set + { + m_fakeisphantom = value; + AddChange(changes.Phantom, 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 void getContactData(ref ContactData cdata) + { + cdata.mu = mu; + cdata.bounce = bounce; + + // cdata.softcolide = m_softcolide; + cdata.softcolide = false; + + if (m_isphysical) + { + ODEDynamics veh; + if (_parent != null) + veh = ((OdePrim)_parent).m_vehicle; + else + veh = m_vehicle; + + if (veh != null && veh.Type != Vehicle.TYPE_NONE) + cdata.mu *= veh.FrictionFactor; +// cdata.mu *= 0; + } + } + + public override float PhysicsCost + { + get + { + return m_physCost; + } + } + + public override float StreamCost + { + get + { + return m_streamCost; + } + } + + 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 + { + uint oldid = m_localID; + m_localID = value; + _parent_scene.changePrimID(this, oldid); + } + } + + public override PhysicsActor ParentActor + { + get + { + if (childPrim) + return _parent; + else + return (PhysicsActor)this; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if (value) + m_isSelected = value; // if true set imediatly to stop moves etc + 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; + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + + public override bool ThrottleUpdates {get;set;} + + 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()) + { + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_fakeShapetype); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return primMass; } + } + + public override Vector3 Force + { + 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) + { + m_fakeisVolumeDetect = (param != 0); + AddChange(changes.VolumeDtc, m_fakeisVolumeDetect); + } + + public override Vector3 GeometricCenter + { + // this is not real geometric center but a average of positions relative to root prim acording to + // http://wiki.secondlife.com/wiki/llGetGeometricCenter + // ignoring tortured prims details since sl also seems to ignore + // so no real use in doing it on physics + get + { + return Vector3.Zero; + } + } + + public override PhysicsInertiaData GetInertiaData() + { + PhysicsInertiaData inertia; + if(childPrim) + { + if(_parent != null) + return _parent.GetInertiaData(); + else + { + inertia = new PhysicsInertiaData(); + inertia.TotalMass = -1; + return inertia; + } + } + + inertia = new PhysicsInertiaData(); + + // double buffering + if(m_fakeInertiaOverride != null) + { + d.Mass objdmass = new d.Mass(); + objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X; + objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y; + objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z; + + objdmass.mass = m_fakeInertiaOverride.TotalMass; + + if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999) + { + d.Matrix3 inertiarotmat = new d.Matrix3(); + d.Quaternion inertiarot = new d.Quaternion(); + + inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X; + inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y; + inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z; + inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W; + d.RfromQ(out inertiarotmat, ref inertiarot); + d.MassRotate(ref objdmass, ref inertiarotmat); + } + + inertia.TotalMass = m_fakeInertiaOverride.TotalMass; + inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass; + inertia.Inertia.X = objdmass.I.M00; + inertia.Inertia.Y = objdmass.I.M11; + inertia.Inertia.Z = objdmass.I.M22; + inertia.InertiaRotation.X = objdmass.I.M01; + inertia.InertiaRotation.Y = objdmass.I.M02; + inertia.InertiaRotation.Z = objdmass.I.M12; + return inertia; + } + + inertia.TotalMass = m_mass; + + if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero) + { + inertia.CenterOfMass = Vector3.Zero; + inertia.Inertia = Vector3.Zero; + inertia.InertiaRotation = Vector4.Zero; + return inertia; + } + + d.Vector3 dtmp; + d.Mass m = new d.Mass(); + lock(_parent_scene.OdeLock) + { + d.AllocateODEDataForThread(0); + dtmp = d.GeomGetOffsetPosition(prim_geom); + d.BodyGetMass(Body, out m); + } + + Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z); + inertia.CenterOfMass = cm; + inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22); + inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0); + + return inertia; + } + + public override void SetInertiaData(PhysicsInertiaData inertia) + { + if(childPrim) + { + if(_parent != null) + _parent.SetInertiaData(inertia); + return; + } + + if(inertia.TotalMass > 0) + m_fakeInertiaOverride = new PhysicsInertiaData(inertia); + else + m_fakeInertiaOverride = null; + + if (inertia.TotalMass > _parent_scene.maximumMassObject) + inertia.TotalMass = _parent_scene.maximumMassObject; + AddChange(changes.SetInertia,(object)m_fakeInertiaOverride); + } + + public override Vector3 CenterOfMass + { + get + { + lock (_parent_scene.OdeLock) + { + d.AllocateODEDataForThread(0); + + d.Vector3 dtmp; + if (!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 Ptot = m_OBBOffset * q; + dtmp = d.GeomGetPosition(prim_geom); + Ptot.X += dtmp.X; + Ptot.Y += dtmp.Y; + Ptot.Z += dtmp.Z; + + // if(childPrim) we only know about physical linksets + return Ptot; +/* + float tmass = _mass; + Ptot *= tmass; + + float m; + + foreach (OdePrim prm in childrenPrim) + { + m = prm._mass; + Ptot += prm.CenterOfMass * m; + tmass += m; + } + + if (tmass == 0) + tmass = 0; + else + tmass = 1.0f / tmass; + + Ptot *= tmass; + return Ptot; +*/ + } + else + return _position; + } + } + } + + public override PrimitiveBaseShape Shape + { + set + { +// AddChange(changes.Shape, value); + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_fakeShapetype); + } + } + + public override byte PhysicsShapeType + { + get + { + return m_fakeShapetype; + } + set + { + m_fakeShapetype = value; + _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value); + } + } + + public override Vector3 rootVelocity + { + get + { + if(_parent != null) + return ((OdePrim)_parent).Velocity; + return Velocity; + } + } + + public override Vector3 Velocity + { + get + { + if (_zeroFlag) + return Vector3.Zero; + return _velocity; + } + set + { + if (value.IsFinite()) + { + if(m_outbounds) + _velocity = value; + else + AddChange(changes.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 m_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++; + + value.Normalize(); + + 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 m_acceleration; } + set + { + if(m_outbounds) + m_acceleration = value; + } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + if(m_outbounds) + m_rotationalVelocity = value; + else + AddChange(changes.AngVelocity, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + AddChange(changes.Buoyancy,value); + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + AddChange(changes.PIDTarget,value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive + { + get + { + return m_usePID; + } + set + { + AddChange(changes.PIDActive,value); + } + } + + public override float PIDTau + { + set + { + float tmp = 0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDTau,tmp); + } + } + + public override float PIDHoverHeight + { + set + { + AddChange(changes.PIDHoverHeight,value); + } + } + public override bool PIDHoverActive + { + get + { + return m_useHoverPID; + } + set + { + AddChange(changes.PIDHoverActive, value); + } + } + + public override PIDHoverType PIDHoverType + { + set + { + AddChange(changes.PIDHoverType,value); + } + } + + public override float PIDHoverTau + { + set + { + float tmp =0; + if (value > 0) + { + float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep); + if (value < mint) + tmp = mint; + else + tmp = value; + } + AddChange(changes.PIDHoverTau, tmp); + } + } + + 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.VehicleRotationParam, fp); + } + + public override void VehicleFlags(int param, bool value) + { + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); + } + + public override void SetVehicle(object vdata) + { + AddChange(changes.SetVehicle, vdata); + } + public void SetAcceleration(Vector3 accel) + { + m_acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + if(pushforce) + AddChange(changes.AddForce, force); + else // a impulse + AddChange(changes.AddForce, force * m_invTimeStep); + } + 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()) + { +// if(pushforce) for now applyrotationimpulse seems more happy applied as a force + AddChange(changes.AddAngForce, force); +// else // a impulse +// AddChange(changes.AddAngForce, force * m_invTimeStep); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + lock(_parent_scene.OdeLock) + { + if (m_outbounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + m_lastposition = _position; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + d.AllocateODEDataForThread(0); + + m_lastVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + if(Body != IntPtr.Zero) + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + if (prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + + m_outbounds = false; + changeDisable(false); + base.RequestPhysicsterseUpdate(); + } + } + } + + public override void CrossingStart() + { + lock(_parent_scene.OdeLock) + { + if (m_outbounds || childPrim) + return; + + m_outbounds = true; + + m_lastposition = _position; + m_lastorientation = _orientation; + + d.AllocateODEDataForThread(0); + if(Body != IntPtr.Zero) + { + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + } + if(prim_geom != IntPtr.Zero) + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + UnSubscribeEvents(); + } + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + mu = _parent_scene.m_materialContactsData[pMaterial].mu; + bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public override float Density + { + get + { + return m_density * 100f; + } + set + { + float old = m_density; + m_density = value / 100f; + // if(m_density != old) + // UpdatePrimBodyData(); + } + } + public override float GravModifier + { + get + { + return m_gravmod; + } + set + { + m_gravmod = value; + if (m_vehicle != null) + m_vehicle.GravMod = m_gravmod; + } + } + public override float Friction + { + get + { + return mu; + } + set + { + mu = value; + } + } + + public override float Restitution + { + get + { + return bounce; + } + set + { + bounce = value; + } + } + + 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(byte axislock) + { +// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axislock); + + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_cureventsubscription = 0; + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + if (CollisionVDTCEventsThisFrame == null) + CollisionVDTCEventsThisFrame = new CollisionEventUpdate(); + SentEmptyCollisionsEvent = false; + } + + public override void UnSubscribeEvents() + { + if (CollisionVDTCEventsThisFrame != null) + { + CollisionVDTCEventsThisFrame.Clear(); + CollisionVDTCEventsThisFrame = null; + } + if (CollisionEventsThisFrame != null) + { + CollisionEventsThisFrame.Clear(); + CollisionEventsThisFrame = null; + } + m_eventsubscription = 0; + _parent_scene.RemoveCollisionEventReporting(this); + } + + public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + _parent_scene.AddCollisionEventReporting(this); + } + + public override void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionVDTCEventsThisFrame == null) + CollisionVDTCEventsThisFrame = new CollisionEventUpdate(); + + CollisionVDTCEventsThisFrame.AddCollider(CollidedWith, contact); + _parent_scene.AddCollisionEventReporting(this); + } + + internal void SleeperAddCollisionEvents() + { + if(CollisionEventsThisFrame != null && CollisionEventsThisFrame.m_objCollisionList.Count != 0) + { + foreach(KeyValuePair kvp in CollisionEventsThisFrame.m_objCollisionList) + { + if(kvp.Key == 0) + continue; + OdePrim other = _parent_scene.getPrim(kvp.Key); + if(other == null) + continue; + ContactPoint cp = kvp.Value; + cp.SurfaceNormal = - cp.SurfaceNormal; + cp.RelativeSpeed = -cp.RelativeSpeed; + other.AddCollisionEvent(ParentActor.LocalID,cp); + } + } + if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.m_objCollisionList.Count != 0) + { + foreach(KeyValuePair kvp in CollisionVDTCEventsThisFrame.m_objCollisionList) + { + OdePrim other = _parent_scene.getPrim(kvp.Key); + if(other == null) + continue; + ContactPoint cp = kvp.Value; + cp.SurfaceNormal = - cp.SurfaceNormal; + cp.RelativeSpeed = -cp.RelativeSpeed; + other.AddCollisionEvent(ParentActor.LocalID,cp); + } + } + } + + internal void clearSleeperCollisions() + { + if(CollisionVDTCEventsThisFrame != null && CollisionVDTCEventsThisFrame.Count >0 ) + CollisionVDTCEventsThisFrame.Clear(); + } + + public void SendCollisions(int timestep) + { + if (m_cureventsubscription < 50000) + m_cureventsubscription += timestep; + + + if (m_cureventsubscription < m_eventsubscription) + return; + + if (CollisionEventsThisFrame == null) + return; + + int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count; + + if (!SentEmptyCollisionsEvent || ncolisions > 0) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + m_cureventsubscription = 0; + + if (ncolisions == 0) + { + SentEmptyCollisionsEvent = true; +// _parent_scene.RemoveCollisionEventReporting(this); + } + else if(Body == IntPtr.Zero || (d.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 )) + { + SentEmptyCollisionsEvent = false; + CollisionEventsThisFrame.Clear(); + } + } + } + + 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,bool pisPhantom,byte _shapeType,uint plocalID) + { + _parent_scene = parent_scene; + + Name = primName; + m_localID = plocalID; + + 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; + + m_timeStep = parent_scene.ODE_STEPSIZE; + m_invTimeStep = 1f / m_timeStep; + + m_density = parent_scene.geomDefaultDensity; + m_body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + collide_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; + + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + m_fakeisVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_colliderfilter = 0; + m_NoColide = false; + + _triMeshData = IntPtr.Zero; + + m_fakeShapetype = _shapeType; + + m_lastdoneSelected = false; + m_isSelected = false; + m_delaySelect = false; + + m_isphantom = pisPhantom; + m_fakeisphantom = pisPhantom; + + mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + + // get basic mass parameters + ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, _shapeType); + + primVolume = repData.volume; + m_OBB = repData.OBB; + m_OBBOffset = repData.OBBOffset; + + UpdatePrimBodyData(); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void UpdateCollisionCatFlags() + { + if(m_isphysical && m_disabled) + { + m_collisionCategories = 0; + m_collisionFlags = 0; + } + + else if (m_isSelected) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + } + + else if (m_isVolumeDetect) + { + m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + m_collisionFlags = 0; + } + else if (m_isphantom) + { + m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + m_collisionFlags = CollisionCategories.Land; + else + m_collisionFlags = 0; + } + else + { + m_collisionCategories = CollisionCategories.Geom; + if (m_isphysical) + m_collisionFlags = m_default_collisionFlagsPhysical; + else + m_collisionFlags = m_default_collisionFlagsNotPhysical; + } + } + + private void ApplyCollisionCatFlags() + { + if (prim_geom != IntPtr.Zero) + { + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + { + if (m_isphysical && m_disabled) + { + prm.m_collisionCategories = 0; + prm.m_collisionFlags = 0; + } + else + { + // preserve some + if (prm.m_isSelected) + { + prm.m_collisionCategories = CollisionCategories.Selected; + prm.m_collisionFlags = 0; + } + else if (prm.m_isVolumeDetect) + { + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character; + else + prm.m_collisionFlags = 0; + } + else if (prm.m_isphantom) + { + prm.m_collisionCategories = CollisionCategories.Phantom; + if (m_isphysical) + prm.m_collisionFlags = CollisionCategories.Land; + else + prm.m_collisionFlags = 0; + } + else + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + } + } + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + if (m_isphysical) + d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); + else + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + } + } + } + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + } + + private void createAMotor(byte axislock) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 0; + bool axisX = false; + bool axisY = false; + bool axisZ = false; + if((axislock & 0x02) != 0) + { + axisnum++; + axisX = true; + } + if((axislock & 0x04) != 0) + { + axisnum++; + axisY = true; + } + if((axislock & 0x08) != 0) + { + axisnum++; + axisZ = true; + } + + 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; + + int i = 0; + int j = 0; + if (axisX) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); + i++; + j = 256; // move to next axis set + } + + if (axisY) + { + 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, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + i++; + j += 256; + } + + if (axisZ) + { + 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, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f); + 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.CFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); + } + } + + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + if (m_isphysical) + { + d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land); + } + else + { + d.GeomSetCollideBits(prim_geom, 0); + d.GeomDisable(prim_geom); + } + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + + UpdatePrimBodyData(); + _parent_scene.actor_name_map[prim_geom] = this; + +/* +// debug + d.AABB aabb; + d.GeomGetAABB(prim_geom, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if( x > 60.0f || y > 60.0f || z > 60.0f) + m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); + else if (x < 0.001f || y < 0.001f || z < 0.001f) + m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}", + Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString()); +*/ + + } + else + m_log.Warn("Setting bad Geom"); + } + + private bool GetMeshGeom() + { + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + IMesh mesh = m_mesh; + + if (mesh == null) + return false; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString()); + + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } + + if (vertexCount > 64000 || indexCount > 64000) + { + m_log.WarnFormat("[PHYSICS]: large mesh data on OdePrim {0}, mesh {1} at {2}, {3} vertices, {4} indexes", + Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", + _position.ToString() ,vertexCount , indexCount ); + } + IntPtr geo = IntPtr.Zero; + + try + { + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + if (_triMeshData != IntPtr.Zero) + { + try + { + d.GeomTriMeshDataDestroy(_triMeshData); + } + catch + { + } + } + _triMeshData = IntPtr.Zero; + + m_hasOBB = false; + m_OBBOffset = Vector3.Zero; + m_OBB = _size * 0.5f; + m_physCost = 0.1f; + m_streamCost = 1.0f; + + _parent_scene.mesher.ReleaseMesh(mesh); + m_meshState = MeshState.MeshFailed; + m_mesh = null; + return false; + } + + m_physCost = 0.0013f * (float)indexCount; + // todo + m_streamCost = 1.0f; + + SetGeom(geo); + + return true; + } + + private void CreateGeom(bool OverrideToBox) + { + bool hasMesh = false; + + m_NoColide = false; + + if ((m_meshState & MeshState.MeshNoColide) != 0) + m_NoColide = true; + + else if(!OverrideToBox && m_mesh != null) + { + if (GetMeshGeom()) + hasMesh = true; + else + m_NoColide = true; + } + + + if (!hasMesh) + { + IntPtr geo = IntPtr.Zero; + + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + try + { + geo = 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 + try + { + geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + m_physCost = 0.1f; + m_streamCost = 1.0f; + SetGeom(geo); + } + } + + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.actor_name_map.Remove(prim_geom); + + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); + } + + prim_geom = IntPtr.Zero; + collide_geom = IntPtr.Zero; + m_targetSpace = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + + lock (m_meshlock) + { + if (m_mesh != null) + { + _parent_scene.mesher.ReleaseMesh(m_mesh); + m_mesh = null; + } + } + + Body = IntPtr.Zero; + m_hasOBB = false; + } + + //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; + collide_geom = IntPtr.Zero; + } + + public void enableBodySoft() + { + m_disabled = false; + if (!childPrim && !m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_isSelected) + m_collisionFlags = CollisionCategories.Selected; + else + m_collisionCategories = 0; + m_collisionFlags = 0; + ApplyCollisionCatFlags(); + 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) + { + DestroyBody(); + 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"); + } + + bool noInertiaOverride = (m_InertiaOverride == null); + + Body = d.BodyCreate(_parent_scene.world); + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.RfromQ(out mymat, ref myrot); + + // set the body rotation + d.BodySetRotation(Body, ref mymat); + + if(noInertiaOverride) + { + objdmass = primdMass; + d.MassRotate(ref objdmass, 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; + rcm.Y = _position.Y; + rcm.Z = _position.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; + } + + 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); + + // 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 + + if(noInertiaOverride) + { + tmpdmass = prm.primdMass; + + d.MassRotate(ref tmpdmass, ref mat); + Vector3 ppos = prm._position; + ppos.X -= rcm.X; + ppos.Y -= rcm.Y; + ppos.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 + } + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + if(noInertiaOverride) + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + else + { + Vector3 ncm = m_InertiaOverride.CenterOfMass * _orientation; + d.BodySetPosition(Body, + _position.X + ncm.X, + _position.Y + ncm.Y, + _position.Z + ncm.Z); + } + + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + if(noInertiaOverride) + { + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.X = -myrot.X; + myrot.Y = -myrot.Y; + myrot.Z = -myrot.Z; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + d.BodySetMass(Body, ref objdmass); + m_mass = objdmass.mass; + } + else + { + objdmass.c.X = 0; + objdmass.c.Y = 0; + objdmass.c.Z = 0; + + objdmass.I.M00 = m_InertiaOverride.Inertia.X; + objdmass.I.M11 = m_InertiaOverride.Inertia.Y; + objdmass.I.M22 = m_InertiaOverride.Inertia.Z; + + objdmass.mass = m_InertiaOverride.TotalMass; + + if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999) + { + d.Matrix3 inertiarotmat = new d.Matrix3(); + d.Quaternion inertiarot = new d.Quaternion(); + + inertiarot.X = m_InertiaOverride.InertiaRotation.X; + inertiarot.Y = m_InertiaOverride.InertiaRotation.Y; + inertiarot.Z = m_InertiaOverride.InertiaRotation.Z; + inertiarot.W = m_InertiaOverride.InertiaRotation.W; + d.RfromQ(out inertiarotmat, ref inertiarot); + d.MassRotate(ref objdmass, ref inertiarotmat); + } + d.BodySetMass(Body, ref objdmass); + + m_mass = objdmass.mass; + } + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodySetAutoDisableAngularThreshold(Body, 0.05f); + d.BodySetAutoDisableLinearThreshold(Body, 0.05f); + d.BodySetDamping(Body, .004f, .001f); + + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + if (childrenPrim.Count == 0) + { + collide_geom = prim_geom; + m_targetSpace = _parent_scene.ActiveSpace; + } + else + { + m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); + d.HashSpaceSetLevels(m_targetSpace, -2, 8); + d.SpaceSetSublevel(m_targetSpace, 3); + d.SpaceSetCleanup(m_targetSpace, false); + + d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(m_targetSpace, 0); + collide_geom = m_targetSpace; + } + + d.SpaceAdd(m_targetSpace, prim_geom); + + if (m_delaySelect) + { + m_isSelected = true; + m_delaySelect = false; + } + + m_collisionscore = 0; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + _parent_scene.addActivePrim(this); + + 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 + + if (prm.m_targetSpace != m_targetSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = m_targetSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + + prm.m_collisionscore = 0; + + if(!m_disabled) + prm.m_disabled = false; + + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if (m_angularlocks != 0 && _parent == null) + { + createAMotor(m_angularlocks); + } + + if (m_isSelected || m_disabled) + { + d.BodyDisable(Body); + _zeroFlag = true; + } + else + { + d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); + d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); + + _zeroFlag = false; + m_bodydisablecontrol = 0; + } + _parent_scene.addActiveGroups(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + + collide_geom = IntPtr.Zero; + + if (m_disabled) + m_collisionCategories = 0; + else if (m_isSelected) + m_collisionCategories = CollisionCategories.Selected; + else if (m_isVolumeDetect) + m_collisionCategories = CollisionCategories.VolumeDtc; + else if (m_isphantom) + m_collisionCategories = CollisionCategories.Phantom; + else + m_collisionCategories = CollisionCategories.Geom; + + m_collisionFlags = 0; + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + } + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + + if (prm.m_isSelected) + prm.m_collisionCategories = CollisionCategories.Selected; + else if (prm.m_isVolumeDetect) + prm.m_collisionCategories = CollisionCategories.VolumeDtc; + else if (prm.m_isphantom) + prm.m_collisionCategories = CollisionCategories.Phantom; + else + prm.m_collisionCategories = CollisionCategories.Geom; + + prm.m_collisionFlags = 0; + + if (prm.prim_geom != IntPtr.Zero) + { + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags); + } + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm.m_mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + _parent_scene.remActiveGroup(this); + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + m_mass = primMass; + m_collisionscore = 0; + } + + private void FixInertia(Vector3 NewPos,Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + + d.BodyGetMass(Body, out tmpdmass); + objdmass = tmpdmass; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + + // transform to object frame + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // back prim own inertia + tmpdmass = primdMass; + + // update to new position and orientation + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + m_mass = objdmass.mass; + } + + private void FixInertia(Vector3 NewPos) + { + d.Matrix3 primmat = new d.Matrix3(); + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Mass primmass = new d.Mass { }; + + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + primmass = primdMass; + // transform to object frame + primmat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref primmass, ref primmat); + + tmpdmass = primmass; + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new position + _position = NewPos; + d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); + + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref primmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref primmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + m_mass = objdmass.mass; + } + + private void FixInertia(Quaternion newrot) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + + d.Mass tmpdmass = new d.Mass { }; + d.Mass objdmass = new d.Mass { }; + d.Vector3 dobjpos; + d.Vector3 thispos; + + d.BodyGetMass(Body, out objdmass); + + // get prim own inertia in its local frame + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + // transform to object frame + thispos = d.GeomGetOffsetPosition(prim_geom); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + // subtract current prim inertia from object + DMassSubPartFromObj(ref tmpdmass, ref objdmass); + + // update to new orientation + _orientation = newrot; + quat.X = newrot.X; + quat.Y = newrot.Y; + quat.Z = newrot.Z; + quat.W = newrot.W; + d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); + + tmpdmass = primdMass; + mat = d.GeomGetOffsetRotation(prim_geom); + d.MassRotate(ref tmpdmass, ref mat); + d.MassTranslate(ref tmpdmass, + thispos.X, + thispos.Y, + thispos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); + + // fix all positions + IntPtr g = d.BodyGetFirstGeom(Body); + while (g != IntPtr.Zero) + { + thispos = d.GeomGetOffsetPosition(g); + thispos.X -= objdmass.c.X; + thispos.Y -= objdmass.c.Y; + thispos.Z -= objdmass.c.Z; + d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); + g = d.dBodyGetNextGeom(g); + } + + d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos); + // get current object position and rotation + dobjpos = d.BodyGetPosition(Body); + + d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + d.BodySetMass(Body, ref objdmass); + m_mass = objdmass.mass; + } + + + #region Mass Calculation + + private void UpdatePrimBodyData() + { + primMass = m_density * primVolume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + m_mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z); + + d.MassTranslate(ref primdMass, + m_OBBOffset.X, + m_OBBOffset.Y, + m_OBBOffset.Z); + + primOOBradiusSQ = m_OBB.LengthSquared(); + + if (_triMeshData != IntPtr.Zero) + { + float pc = m_physCost; + float psf = primOOBradiusSQ; + psf *= 1.33f * .2f; + pc *= psf; + if (pc < 0.1f) + pc = 0.1f; + + m_physCost = pc; + } + else + m_physCost = 0.1f; + + m_streamCost = 1.0f; + } + + #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.Quaternion qtmp; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + _orientation.W = qtmp.W; +/* +// Debug + float qlen = _orientation.Length(); + if (qlen > 1.01f || qlen < 0.99) + m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen); +// +*/ + _orientation.Normalize(); + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.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() + { + _parent_scene.addToPrims(this); + } + + private void changeAngularLock(byte newLocks) + { + // 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 (newLocks != 0) + { + createAMotor(newLocks); + } + 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_angularlocks = newLocks; + } + + 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; +// m_torque = Vector3.Zero; + _velocity = Vector3.Zero; + m_acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + _zeroFlag = false; + base.RequestPhysicsterseUpdate(); + } + + 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 changePhantomStatus(bool newval) + { + m_isphantom = newval; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + +/* not in use + internal void ChildSelectedChange(bool childSelect) + { + if(childPrim) + return; + + if (childSelect == m_isSelected) + return; + + if (childSelect) + { + DoSelectedStatus(true); + } + + else + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_isSelected) + return; + } + DoSelectedStatus(false); + } + } +*/ + private void changeSelectedStatus(bool newval) + { + if (m_lastdoneSelected == newval) + return; + + m_lastdoneSelected = newval; + DoSelectedStatus(newval); + } + + private void CheckDelaySelect() + { + if (m_delaySelect) + { + DoSelectedStatus(m_isSelected); + } + } + + private void DoSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + if (!childPrim && Body != IntPtr.Zero) + d.BodyDisable(Body); + + if (m_delaySelect || m_isphysical) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = 0; + + if (!childPrim) + { + foreach (OdePrim prm in childrenPrim) + { + prm.m_collisionCategories = m_collisionCategories; + prm.m_collisionFlags = m_collisionFlags; + + if (prm.prim_geom != IntPtr.Zero) + { + + if (prm.m_NoColide) + { + d.GeomSetCategoryBits(prm.prim_geom, 0); + d.GeomSetCollideBits(prm.prim_geom, 0); + } + else + { + d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags); + } + } + prm.m_delaySelect = false; + } + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(true); + + + if (prim_geom != IntPtr.Zero) + { + if (m_NoColide) + { + d.GeomSetCategoryBits(prim_geom, 0); + d.GeomSetCollideBits(prim_geom, 0); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, 0); + d.GeomSetCollideBits(collide_geom, 0); + } + + } + else + { + d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags); + if (collide_geom != prim_geom && collide_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories); + d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags); + } + } + } + + m_delaySelect = false; + } + else if(!m_isphysical) + { + m_delaySelect = true; + } + } + else + { + if (!childPrim) + { + if (Body != IntPtr.Zero && !m_disabled) + { + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } +// else if (_parent != null) +// ((OdePrim)_parent).ChildSelectedChange(false); + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + + m_delaySelect = false; + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + + else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) + { + FixInertia(newPos); + if (!d.BodyIsEnabled(Body)) + { + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + { + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + 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) + { + CheckDelaySelect(); + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } +/* + else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) + { + FixInertia(_position, newOri); + if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } +*/ + } + 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) + { + if(m_angularlocks != 0) + createAMotor(m_angularlocks); + } + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + { + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + 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) + { + CheckDelaySelect(); + 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_angularlocks != 0) + createAMotor(m_angularlocks); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + { + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + 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) + { + CheckDelaySelect(); + + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + MakeBody(); + } + else + { + if (Body != IntPtr.Zero) + { + DestroyBody(); + } + Stop(); + } + } + + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + } + + private void changeShape(PrimitiveBaseShape newShape) + { + } + + private void changeAddPhysRep(ODEPhysRepData repData) + { + _size = repData.size; //?? + _pbs = repData.pbs; + + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_meshState = repData.meshState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(repData.isTooSmall); + + 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 (!m_isphysical) + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + else + MakeBody(); + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_fakeShapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); + } + else + m_shapetype = repData.shapetype; + } + + private void changePhysRepData(ODEPhysRepData repData) + { + if(_size == repData.size && + _pbs == repData.pbs && + m_shapetype == repData.shapetype && + m_mesh == repData.mesh && + primVolume == repData.volume) + return; + + CheckDelaySelect(); + + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + _size = repData.size; + _pbs = repData.pbs; + + m_mesh = repData.mesh; + + m_assetID = repData.assetID; + m_meshState = repData.meshState; + + m_hasOBB = repData.hasOBB; + m_OBBOffset = repData.OBBOffset; + m_OBB = repData.OBB; + + primVolume = repData.volume; + + CreateGeom(repData.isTooSmall); + + 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 (m_isphysical) + { + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + } + else + { + SetInStaticSpace(this); + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + resetCollisionAccounting(); + + if ((m_meshState & MeshState.NeedMask) != 0) + { + repData.size = _size; + repData.pbs = _pbs; + repData.shapetype = m_fakeShapetype; + _parent_scene.m_meshWorker.RequestMesh(repData); + } + else + m_shapetype = repData.shapetype; + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected && !m_outbounds) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + m_torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + + private void changeAddForce(Vector3 theforce) + { + m_forceacc += theforce; + if (!m_isSelected && !m_outbounds) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + } + m_collisionscore = 0; + } + } + + // actually angular impulse + private void changeAddAngularImpulse(Vector3 aimpulse) + { + m_angularForceacc += aimpulse * m_invTimeStep; + if (!m_isSelected && !m_outbounds) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + } + } + m_collisionscore = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + float len = newVel.LengthSquared(); + if (len > 100000.0f) // limit to 100m/s + { + len = 100.0f / (float)Math.Sqrt(len); + newVel *= len; + } + + if (!m_isSelected && !m_outbounds) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeangvelocity(Vector3 newAngVel) + { + float len = newAngVel.LengthSquared(); + if (len > _parent_scene.maxAngVelocitySQ) + { + len = _parent_scene.maximumAngularVelocity / (float)Math.Sqrt(len); + newAngVel *= len; + } + + if (!m_isSelected && !m_outbounds) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + { + d.BodySetAutoDisableSteps(Body, m_body_autodisable_frames); + d.BodyEnable(Body); + } + d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z); + } + //resetCollisionAccounting(); + } + m_rotationalVelocity = newAngVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + m_fakeisVolumeDetect = newVolDtc; + UpdateCollisionCatFlags(); + ApplyCollisionCatFlags(); + } + + protected void changeBuilding(bool newbuilding) + { + // Check if we need to do anything + if (newbuilding == m_building) + return; + + if ((bool)newbuilding) + { + m_building = true; + if (!childPrim) + DestroyBody(); + } + else + { + m_building = false; + CheckDelaySelect(); + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + public void changeSetVehicle(VehicleData vdata) + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + m_vehicle.DoSetVehicle(vdata); + } + + private void changeVehicleType(int value) + { + if (value == (int)Vehicle.TYPE_NONE) + { + if (m_vehicle != null) + m_vehicle = null; + } + else + { + if (m_vehicle == null) + m_vehicle = new ODEDynamics(this); + + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + } + + private void changeBuoyancy(float b) + { + m_buoyancy = b; + } + + private void changePIDTarget(Vector3 trg) + { + m_PIDTarget = trg; + } + + private void changePIDTau(float tau) + { + m_PIDTau = tau; + } + + private void changePIDActive(bool val) + { + m_usePID = val; + } + + private void changePIDHoverHeight(float val) + { + m_PIDHoverHeight = val; + if (val == 0) + m_useHoverPID = false; + } + + private void changePIDHoverType(PIDHoverType type) + { + m_PIDHoverType = type; + } + + private void changePIDHoverTau(float tau) + { + m_PIDHoverTau = tau; + } + + private void changePIDHoverActive(bool active) + { + m_useHoverPID = active; + } + + private void changeInertia(PhysicsInertiaData inertia) + { + m_InertiaOverride = inertia; + + if (Body != IntPtr.Zero) + DestroyBody(); + MakeBody(); + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && !m_building && !m_outbounds) + { + if (!d.BodyIsEnabled(Body)) + { + // let vehicles sleep + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return; + + if (++m_bodydisablecontrol < 50) + return; + + // clear residuals + d.BodySetAngularVel(Body,0f,0f,0f); + d.BodySetLinearVel(Body,0f,0f,0f); + _zeroFlag = true; + d.BodySetAutoDisableSteps(Body, 1); + d.BodyEnable(Body); + m_bodydisablecontrol = -3; + } + + if(m_bodydisablecontrol < 0) + m_bodydisablecontrol++; + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + return; + } + + float fx = 0; + float fy = 0; + float fz = 0; + + float mass = m_mass; + + if (m_usePID && m_PIDTau > 0) + { + // for now position error + _target_velocity = + new Vector3( + (m_PIDTarget.X - lpos.X), + (m_PIDTarget.Y - lpos.Y), + (m_PIDTarget.Z - lpos.Z) + ); + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f)) + { + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + return; + } + else + { + _zeroFlag = false; + + float tmp = 1 / m_PIDTau; + _target_velocity *= tmp; + + // apply limits + tmp = _target_velocity.Length(); + if (tmp > 50.0f) + { + tmp = 50 / tmp; + _target_velocity *= tmp; + } + else if (tmp < 0.05f) + { + tmp = 0.05f / tmp; + _target_velocity *= tmp; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + fx = (_target_velocity.X - vel.X) * m_invTimeStep; + fy = (_target_velocity.Y - vel.Y) * m_invTimeStep; + fz = (_target_velocity.Z - vel.Z) * m_invTimeStep; +// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0) + { + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y); + + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + + case PIDHoverType.GroundAndWater: + 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) + + // don't go underground unless volumedetector + + if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + fz = (m_targetHoverHeight - lpos.Z); + + // if error is zero, use position control; otherwise, velocity control + if (Math.Abs(fz) < 0.01f) + { + d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + } + else + { + _zeroFlag = false; + fz /= m_PIDHoverTau; + + float tmp = Math.Abs(fz); + if (tmp > 50) + fz = 50 * Math.Sign(fz); + else if (tmp < 0.1) + fz = 0.1f * Math.Sign(fz); + + fz = ((fz - vel.Z) * m_invTimeStep); + } + } + } + else + { + float b = (1.0f - m_buoyancy) * m_gravmod; + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= mass; + fy *= mass; + fz *= 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 = m_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(int frame) + { + if (_parent == null && !m_isSelected && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero) + { + if(m_bodydisablecontrol < 0) + return; + + bool bodyenabled = d.BodyIsEnabled(Body); + if (bodyenabled || !_zeroFlag) + { + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos = d.GeomGetPosition(prim_geom); + + // check outside region + if (lpos.Z < -100 || lpos.Z > 100000f) + { + m_outbounds = true; + + lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); + m_acceleration.X = 0; + m_acceleration.Y = 0; + m_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(); + +// throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + return; + } + + if (lpos.X < 0f) + { + _position.X = Util.Clip(lpos.X, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.X > _parent_scene.WorldExtents.X) + { + _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); + m_outbounds = true; + } + if (lpos.Y < 0f) + { + _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); + m_outbounds = true; + } + else if (lpos.Y > _parent_scene.WorldExtents.Y) + { + _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); + m_outbounds = true; + } + + if (m_outbounds) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + d.Vector3 dtmp = d.BodyGetAngularVel(Body); + m_rotationalVelocity.X = dtmp.X; + m_rotationalVelocity.Y = dtmp.Y; + m_rotationalVelocity.Z = dtmp.Z; + + dtmp = d.BodyGetLinearVel(Body); + _velocity.X = dtmp.X; + _velocity.Y = dtmp.Y; + _velocity.Z = dtmp.Z; + + 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 + UnSubscribeEvents(); + + base.RequestPhysicsterseUpdate(); + return; + } + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + + // decide if moving + // use positions since this are integrated quantities + // tolerance values depende a lot on simulation noise... + // use simple math.abs since we dont need to be exact + if(!bodyenabled) + { + _zeroFlag = true; + } + else + { + float poserror; + float angerror; + if(_zeroFlag) + { + poserror = 0.01f; + angerror = 0.001f; + } + else + { + poserror = 0.005f; + angerror = 0.0005f; + } + + if ( + (Math.Abs(_position.X - lpos.X) < poserror) + && (Math.Abs(_position.Y - lpos.Y) < poserror) + && (Math.Abs(_position.Z - lpos.Z) < poserror) + && (Math.Abs(_orientation.X - ori.X) < angerror) + && (Math.Abs(_orientation.Y - ori.Y) < angerror) + && (Math.Abs(_orientation.Z - ori.Z) < angerror) // ignore W + ) + _zeroFlag = true; + else + _zeroFlag = false; + } + + // update position + if (!(_zeroFlag && lastZeroFlag)) + { + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + } + + // update velocities and acceleration + if (_zeroFlag || lastZeroFlag) + { + // disable interpolators + _velocity = Vector3.Zero; + m_acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + } + else + { + d.Vector3 vel = d.BodyGetLinearVel(Body); + + m_acceleration = _velocity; + + if ((Math.Abs(vel.X) < 0.005f) && + (Math.Abs(vel.Y) < 0.005f) && + (Math.Abs(vel.Z) < 0.005f)) + { + _velocity = Vector3.Zero; + float t = -m_invTimeStep; + m_acceleration = m_acceleration * t; + } + else + { + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + m_acceleration = (_velocity - m_acceleration) * m_invTimeStep; + } + + if ((Math.Abs(m_acceleration.X) < 0.01f) && + (Math.Abs(m_acceleration.Y) < 0.01f) && + (Math.Abs(m_acceleration.Z) < 0.01f)) + { + m_acceleration = Vector3.Zero; + } + + vel = d.BodyGetAngularVel(Body); + if ((Math.Abs(vel.X) < 0.0001) && + (Math.Abs(vel.Y) < 0.0001) && + (Math.Abs(vel.Z) < 0.0001) + ) + { + m_rotationalVelocity = Vector3.Zero; + } + else + { + m_rotationalVelocity.X = vel.X; + m_rotationalVelocity.Y = vel.Y; + m_rotationalVelocity.Z = vel.Z; + } + } + + if (_zeroFlag) + { + if (!m_lastUpdateSent) + { + base.RequestPhysicsterseUpdate(); + if (lastZeroFlag) + m_lastUpdateSent = true; + } + return; + } + + base.RequestPhysicsterseUpdate(); + m_lastUpdateSent = false; + } + } + } + + 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 DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) + { + // assumes object center of mass is zero + float smass = part.mass; + theobj.mass -= smass; + + smass *= 1.0f / (theobj.mass); ; + + theobj.c.X -= part.c.X * smass; + theobj.c.Y -= part.c.Y * smass; + theobj.c.Z -= part.c.Z * smass; + + theobj.I.M00 -= part.I.M00; + theobj.I.M01 -= part.I.M01; + theobj.I.M02 -= part.I.M02; + theobj.I.M10 -= part.I.M10; + theobj.I.M11 -= part.I.M11; + theobj.I.M12 -= part.I.M12; + theobj.I.M20 -= part.I.M20; + theobj.I.M21 -= part.I.M21; + theobj.I.M22 -= part.I.M22; + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + + case changes.AddPhysRep: + changeAddPhysRep((ODEPhysRepData)arg); + 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); + + m_vehicle = null; + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + 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.TargetVelocity: + 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: + changeAddAngularImpulse((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((byte)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.PhysRepData: + changePhysRepData((ODEPhysRepData) arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Phantom: + changePhantomStatus((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.SetVehicle: + changeSetVehicle((VehicleData) arg); + break; + + case changes.Buoyancy: + changeBuoyancy((float)arg); + break; + + case changes.PIDTarget: + changePIDTarget((Vector3)arg); + break; + + case changes.PIDTau: + changePIDTau((float)arg); + break; + + case changes.PIDActive: + changePIDActive((bool)arg); + break; + + case changes.PIDHoverHeight: + changePIDHoverHeight((float)arg); + break; + + case changes.PIDHoverType: + changePIDHoverType((PIDHoverType)arg); + break; + + case changes.PIDHoverTau: + changePIDHoverTau((float)arg); + break; + + case changes.PIDHoverActive: + changePIDHoverActive((bool)arg); + break; + + case changes.SetInertia: + changeInertia((PhysicsInertiaData) 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; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs new file mode 100644 index 0000000..f671722 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODERayCastRequestManager.cs @@ -0,0 +1,689 @@ +/* + * 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 OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// 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; // the ray. we only need one for our lifetime + IntPtr Sphere; + IntPtr Box; + IntPtr Plane; + + private int CollisionContactGeomsPerTest = 25; + private const int DefaultMaxCount = 25; + private const int MaxTimePerCallMS = 30; + + /// + /// 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(); + private RayFilterFlags CurrentRayFilter; + private int CurrentMaxCount; + + public ODERayCastRequestManager(ODEScene pScene) + { + m_scene = pScene; + nearCallback = near; + ray = d.CreateRay(IntPtr.Zero, 1.0f); + d.GeomSetCategoryBits(ray, 0); + Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f); + d.GeomSetCategoryBits(Box, 0); + Sphere = d.CreateSphere(IntPtr.Zero,1.0f); + d.GeomSetCategoryBits(Sphere, 0); + Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f); + d.GeomSetCategoryBits(Sphere, 0); + } + + public void QueueRequest(ODERayRequest req) + { + if (req.Count == 0) + req.Count = DefaultMaxCount; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + + if (m_PendingRequests.Count <= 0) + return 0; + + if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero) + // oops something got wrong or scene isn't ready still + { + m_PendingRequests.Clear(); + return 0; + } + + int time = Util.EnvironmentTickCount(); + + ODERayRequest req; + int closestHit; + int backfacecull; + CollisionCategories catflags; + + while (m_PendingRequests.Dequeue(out req)) + { + if (req.callbackMethod != null) + { + IntPtr geom = IntPtr.Zero; + if (req.actor != null) + { + if (m_scene.haveActor(req.actor)) + { + if (req.actor is OdePrim) + geom = ((OdePrim)req.actor).prim_geom; + else if (req.actor is OdeCharacter) + geom = ((OdePrim)req.actor).prim_geom; + } + if (geom == IntPtr.Zero) + { + NoContacts(req); + continue; + } + } + + CurrentRayFilter = req.filter; + CurrentMaxCount = req.Count; + + CollisionContactGeomsPerTest = req.Count & 0xffff; + + closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1); + backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1); + + if (req.callbackMethod is ProbeBoxCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z); + d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z); + d.Quaternion qtmp; + qtmp.X = req.orientation.X; + qtmp.Y = req.orientation.Y; + qtmp.Z = req.orientation.Z; + qtmp.W = req.orientation.W; + d.GeomSetQuaternion(Box, ref qtmp); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomSphereSetRadius(Sphere, req.length); + d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + if (CollisionContactGeomsPerTest > 80) + CollisionContactGeomsPerTest = 80; + + d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length); + } + + else + { + if (CollisionContactGeomsPerTest > 25) + CollisionContactGeomsPerTest = 25; + + 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); + d.GeomRaySetParams(ray, 0, backfacecull); + + if (req.callbackMethod is RaycastCallback) + { + // if we only want one get only one per Collision pair saving memory + CurrentRayFilter |= RayFilterFlags.ClosestHit; + d.GeomRaySetClosestHit(ray, 1); + } + else + d.GeomRaySetClosestHit(ray, closestHit); + } + + if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0) + unchecked + { + CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT; + } + + if (geom == IntPtr.Zero) + { + // translate ray filter to Collision flags + catflags = 0; + if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0) + catflags |= CollisionCategories.VolumeDtc; + if ((CurrentRayFilter & RayFilterFlags.phantom) != 0) + catflags |= CollisionCategories.Phantom; + if ((CurrentRayFilter & RayFilterFlags.agent) != 0) + catflags |= CollisionCategories.Character; + if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0) + catflags |= CollisionCategories.Geom; + if ((CurrentRayFilter & RayFilterFlags.land) != 0) + catflags |= CollisionCategories.Land; + if ((CurrentRayFilter & RayFilterFlags.water) != 0) + catflags |= CollisionCategories.Water; + + if (catflags != 0) + { + if (req.callbackMethod is ProbeBoxCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Box, (uint)catflags); + d.GeomSetCategoryBits(Box, (uint)catflags); + doProbe(req, Box); + } + else if (req.callbackMethod is ProbeSphereCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Sphere, (uint)catflags); + d.GeomSetCategoryBits(Sphere, (uint)catflags); + doProbe(req, Sphere); + } + else if (req.callbackMethod is ProbePlaneCallback) + { + catflags |= CollisionCategories.Space; + d.GeomSetCollideBits(Plane, (uint)catflags); + d.GeomSetCategoryBits(Plane, (uint)catflags); + doPlane(req,IntPtr.Zero); + } + else + { + d.GeomSetCollideBits(ray, (uint)catflags); + doSpaceRay(req); + } + } + } + else + { + // if we select a geom don't use filters + + if (req.callbackMethod is ProbePlaneCallback) + { + d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All); + doPlane(req,geom); + } + else + { + d.GeomSetCollideBits(ray, (uint)CollisionCategories.All); + doGeomRay(req,geom); + } + } + } + + if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS) + break; + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return Util.EnvironmentTickCountSubtract(time); + } + /// + /// Method that actually initiates the raycast with spaces + /// + /// + /// + + private void NoContacts(ODERayRequest req) + { + if (req.callbackMethod is RaycastCallback) + { + ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero); + return; + } + List cresult = new List(); + + if (req.callbackMethod is RayCallback) + ((RayCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom; +// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton; + private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom; + + private void doSpaceRay(ODERayRequest req) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + { + // current ode land to ray collisions is very bad + // so for now limit its range badly + if (req.length > 60.0f) + { + Vector3 t = req.Normal * req.length; + float tmp = t.X * t.X + t.Y * t.Y; + if(tmp > 2500) + { + float tmp2 = req.length * req.length - tmp + 2500; + tmp2 = (float)Math.Sqrt(tmp2); + d.GeomRaySetLength(ray, tmp2); + } + + } + d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if(cResult.Depth < distance) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + if (distance > 0 && distance < float.MaxValue) + hitYN = true; + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); + } + } + + private void doProbe(ODERayRequest req, IntPtr probe) + { + // Collide tests + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + if (req.callbackMethod is ProbeBoxCallback) + ((ProbeBoxCallback)req.callbackMethod)(cresult); + else if (req.callbackMethod is ProbeSphereCallback) + ((ProbeSphereCallback)req.callbackMethod)(cresult); + } + + private void doPlane(ODERayRequest req,IntPtr geom) + { + // Collide tests + if (geom == IntPtr.Zero) + { + if ((CurrentRayFilter & FilterActiveSpace) != 0) + { + d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback); + d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback); + } + if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback); + if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount)) + d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback); + } + else + { + d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback); + } + + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + + ((ProbePlaneCallback)req.callbackMethod)(cresult); + } + + /// + /// Method that actually initiates the raycast with a geom + /// + /// + private void doGeomRay(ODERayRequest req, IntPtr geom) + { + // Collide test + d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = float.MaxValue; + Vector3 closestcontact = Vector3.Zero; + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if(cResult.Depth < distance ) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + if (distance > 0 && distance < float.MaxValue) + hitYN = true; + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + List cresult = new List(m_contactResults.Count); + lock (m_PendingRequests) + { + cresult.AddRange(m_contactResults); + m_contactResults.Clear(); + } + ((RayCallback)req.callbackMethod)(cresult); + } + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; + if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest) + 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. g1 is the ray + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + if (g2 == IntPtr.Zero || g1 == g2) + return; + + if (m_contactResults.Count >= CurrentMaxCount) + return; + + if (d.GeomIsSpace(g2)) + { + 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, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; +/* + uint cat1 = d.GeomGetCategoryBits(g1); + uint cat2 = d.GeomGetCategoryBits(g2); + uint col1 = d.GeomGetCollideBits(g1); + uint col2 = d.GeomGetCollideBits(g2); +*/ + + uint ID = 0; + PhysicsActor p2 = null; + + m_scene.actor_name_map.TryGetValue(g2, out p2); + + if (p2 == null) + return; + + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Prim: + + RayFilterFlags thisFlags; + + if (p2.IsPhysical) + thisFlags = RayFilterFlags.physical; + else + thisFlags = RayFilterFlags.nonphysical; + + if (p2.Phantom) + thisFlags |= RayFilterFlags.phantom; + + if (p2.IsVolumeDtc) + thisFlags |= RayFilterFlags.volumedtc; + + if ((thisFlags & CurrentRayFilter) == 0) + return; + + ID = ((OdePrim)p2).LocalID; + break; + + case (int)ActorTypes.Agent: + + if ((CurrentRayFilter & RayFilterFlags.agent) == 0) + return; + else + ID = ((OdeCharacter)p2).LocalID; + break; + + case (int)ActorTypes.Ground: + + if ((CurrentRayFilter & RayFilterFlags.land) == 0) + return; + break; + + case (int)ActorTypes.Water: + + if ((CurrentRayFilter & RayFilterFlags.water) == 0) + return; + break; + + default: + break; + } + + d.ContactGeom curcontact = new d.ContactGeom(); + + // closestHit for now only works for meshs, so must do it for others + if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0) + { + // Loop all contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; + collisionresult.Depth = curcontact.depth; + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; + lock (m_contactResults) + { + m_contactResults.Add(collisionresult); + if (m_contactResults.Count >= CurrentMaxCount) + return; + } + } + } + else + { + // keep only closest contact + ContactResult collisionresult = new ContactResult(); + collisionresult.ConsumerID = ID; + collisionresult.Depth = float.MaxValue; + + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + + if (curcontact.depth < collisionresult.Depth) + { + collisionresult.Pos.X = curcontact.pos.X; + collisionresult.Pos.Y = curcontact.pos.Y; + collisionresult.Pos.Z = curcontact.pos.Z; + collisionresult.Depth = curcontact.depth; + collisionresult.Normal.X = curcontact.normal.X; + collisionresult.Normal.Y = curcontact.normal.Y; + collisionresult.Normal.Z = curcontact.normal.Z; + } + } + + if (collisionresult.Depth != float.MaxValue) + { + 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; + if (ray != IntPtr.Zero) + { + d.GeomDestroy(ray); + ray = IntPtr.Zero; + } + if (Box != IntPtr.Zero) + { + d.GeomDestroy(Box); + Box = IntPtr.Zero; + } + if (Sphere != IntPtr.Zero) + { + d.GeomDestroy(Sphere); + Sphere = IntPtr.Zero; + } + if (Plane != IntPtr.Zero) + { + d.GeomDestroy(Plane); + Plane = IntPtr.Zero; + } + } + } + + public struct ODERayRequest + { + public PhysicsActor actor; + public Vector3 Origin; + public Vector3 Normal; + public int Count; + public float length; + public object callbackMethod; + public RayFilterFlags filter; + public Quaternion orientation; + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs new file mode 100644 index 0000000..0b51820 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -0,0 +1,2824 @@ +/* + * 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/13 by Ubit Umarov +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using Mono.Addins; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + // colision flags of things others can colide with + // rays, sensors, probes removed since can't be colided with + // The top space where things are placed provided further selection + // ie physical are in active space nonphysical in static + // this should be exclusive as possible + + [Flags] + public enum CollisionCategories : uint + { + Disabled = 0, + //by 'things' types + Space = 0x01, + Geom = 0x02, // aka prim/part + Character = 0x04, + Land = 0x08, + Water = 0x010, + + // by state + Phantom = 0x01000, + VolumeDtc = 0x02000, + Selected = 0x04000, + NoShape = 0x08000, + + + All = 0xffffffff + } + + /// + /// 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, + TargetVelocity, + AngVelocity, + Acceleration, + Force, + Torque, + Momentum, + + AddForce, + AddAngForce, + AngLock, + + Buoyancy, + + PIDTarget, + PIDTau, + PIDActive, + + PIDHoverHeight, + PIDHoverType, + PIDHoverTau, + PIDHoverActive, + + Size, + AvatarSize, + Shape, + PhysRepData, + AddPhysRep, + + CollidesWater, + VolumeDtc, + + Physical, + Phantom, + Selected, + disabled, + building, + + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + SetVehicle, + SetInertia, + + 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 static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + public bool m_OSOdeLib = false; + public Scene m_frameWorkScene = null; + +// private int threadid = 0; + +// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + + const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; + const float comumContactERP = 0.75f; + const float comumContactCFM = 0.0001f; + const float comumContactSLIP = 0f; + + float frictionMovementMult = 0.8f; + + float TerrainBounce = 0.001f; + float TerrainFriction = 0.3f; + + public float AvatarFriction = 0;// 0.9f * 0.5f; + + // this netx dimensions are only relevant for terrain partition (mega regions) + // WorldExtents below has the simulation dimensions + // they should be identical except on mega regions + private uint m_regionWidth = Constants.RegionSize; + private uint m_regionHeight = Constants.RegionSize; + + public float ODE_STEPSIZE = 0.020f; + public float HalfOdeStep = 0.01f; + public int odetimestepMS = 20; // rounded + private float metersInSpace = 25.6f; + private float m_timeDilation = 1.0f; + + private double m_lastframe; + private double m_lastMeshExpire; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + private float waterlevel = 0f; + private int framecount = 0; + + private float avDensity = 80f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + public float geomDefaultDensity = 10.0f; + + public float maximumAngularVelocity = 12.0f; // default 12rad/s + public float maxAngVelocitySQ = 144f; // squared value + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int bodyFramesAutoDisable = 5; + + private d.NearCallback nearCallback; + + private Dictionary _prims = new Dictionary(); + private HashSet _characters = new HashSet(); + private HashSet _activeprims = new HashSet(); + private HashSet _activegroups = new HashSet(); + + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + + /// + /// A list of actors that should receive collision events. + /// + private List _collisionEventPrim = new List(); + private List _collisionEventPrimRemove = new List(); + + private HashSet _badCharacter = new HashSet(); + 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; + private d.Contact SharedTmpcontact = new d.Contact(); + + const int maxContactsbeforedeath = 6000; + private volatile int m_global_contactcount = 0; + + private IntPtr contactgroup; + + public ContactData[] m_materialContactsData = new ContactData[8]; + + private IntPtr TerrainGeom; + private float[] TerrainHeightFieldHeight; + private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle(); + + private int m_physicsiterations = 15; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag +// private PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; + + public IntPtr world; + + // 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 CharsSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + public IntPtr GroundSpace; // space for ground + + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeterX; + private float spacesPerMeterY; + + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + private IntPtr[] staticPrimspaceOffRegion; + + public Object OdeLock; + public static Object SimulationLock; + + public IMesher mesher; + + public IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + + private ODERayCastRequestManager m_rayCastManager; + public ODEMeshWorker m_meshWorker; + + /* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ + + IConfig physicsconfig = null; + + public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib) + { + OdeLock = new Object(); + + EngineType = pname; + PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName; + EngineName = pname + " " + pversion; + m_config = psourceconfig; + m_OSOdeLib = pOSOdeLib; + +// m_OSOdeLib = false; //debug + + m_frameWorkScene = pscene; + + m_frameWorkScene.RegisterModuleInterface(this); + + Initialization(); + + base.Initialise(m_frameWorkScene.PhysicsRequestAsset, + (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]), + (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight); + } + + public void RegionLoaded() + { + mesher = m_frameWorkScene.RequestModuleInterface(); + if (mesher == null) + { + m_log.ErrorFormat("[ubOde] No mesher. module disabled"); + return; + } + + m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig); + m_frameWorkScene.PhysicsEnabled = true; + } + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + private void Initialization() + { + d.AllocateODEDataForThread(~0U); + + SimulationLock = new Object(); + + nearCallback = near; + + m_rayCastManager = new ODERayCastRequestManager(this); + + WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX; + m_regionWidth = (uint)WorldExtents.X; + WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY; + m_regionHeight = (uint)WorldExtents.Y; + + 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); + CharsSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + GroundSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + // i did! + } + + d.HashSpaceSetLevels(TopSpace, -5, 12); + d.HashSpaceSetLevels(ActiveSpace, -5, 10); + d.HashSpaceSetLevels(CharsSpace, -4, 3); + d.HashSpaceSetLevels(StaticSpace, -5, 12); + d.HashSpaceSetLevels(GroundSpace, 0, 8); + + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(CharsSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + d.SpaceSetSublevel(GroundSpace, 1); + + d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Character | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(CharsSpace, 0); + + d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + // CollisionCategories.Land | + // CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(StaticSpace, 0); + + d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundSpace, 0); + + contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + } + + + // checkThread(); + + + // Defaults + + int contactsPerCollision = 80; + + physicsconfig = null; + + if (m_config != null) + { + physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); + gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); + gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); + + // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); + + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); +// bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); + + 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", minimumGroundFlightOffset); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); + + avDensity *= 3f / 80f; // scale other engines density option to this + } + } + + float heartbeat = 1/m_frameWorkScene.FrameTime; + maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI; + maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity; + + d.WorldSetCFM(world, comumContactCFM); + d.WorldSetERP(world, comumContactERP); + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + + d.WorldSetLinearDamping(world, 0.001f); + d.WorldSetAngularDamping(world, 0.002f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity); + + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + d.WorldSetContactMaxCorrectingVel(world, 60.0f); + + HalfOdeStep = ODE_STEPSIZE * 0.5f; + odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f); + + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf); + + SharedTmpcontact.geom.g1 = IntPtr.Zero; + SharedTmpcontact.geom.g2 = IntPtr.Zero; + + SharedTmpcontact.geom.side1 = -1; + SharedTmpcontact.geom.side2 = -1; + + SharedTmpcontact.surface.mode = comumContactFlags; + SharedTmpcontact.surface.mu = 0; + SharedTmpcontact.surface.bounce = 0; + SharedTmpcontact.surface.bounce_vel = 1.5f; + SharedTmpcontact.surface.soft_cfm = comumContactCFM; + SharedTmpcontact.surface.soft_erp = comumContactERP; + SharedTmpcontact.surface.slip1 = comumContactSLIP; + SharedTmpcontact.surface.slip2 = comumContactSLIP; + + m_materialContactsData[(int)Material.Stone].mu = 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; + + m_materialContactsData[(int)Material.Metal].mu = 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = 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; + + + spacesPerMeterX = 1.0f / metersInSpace; + spacesPerMeterY = spacesPerMeterX; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); + + if (spaceGridMaxX > 24) + { + spaceGridMaxX = 24; + spacesPerMeterX = spaceGridMaxX / WorldExtents.X; + } + + if (spaceGridMaxY > 24) + { + spaceGridMaxY = 24; + spacesPerMeterY = spaceGridMaxY / WorldExtents.Y; + } + + 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); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspace[i, j] = newspace; + } + + // let this now be index limit + spaceGridMaxX--; + spaceGridMaxY--; + + // create 4 off world spaces (x<0,x>max,y<0,y>max) + staticPrimspaceOffRegion = new IntPtr[4]; + + for (i = 0; i < 4; i++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | + CollisionCategories.Geom | + CollisionCategories.Land | + CollisionCategories.Water | + CollisionCategories.Phantom | + CollisionCategories.VolumeDtc + )); + d.GeomSetCollideBits(newspace, 0); + + staticPrimspaceOffRegion[i] = newspace; + } + + m_lastframe = Util.GetTimeStamp(); + m_lastMeshExpire = m_lastframe; + step_time = -1; + } + + 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,bool smooth) + { + if (m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; + + m_global_contactcount++; + if(smooth) + SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f; + else + SharedTmpcontact.geom.depth = contactGeom.depth; + SharedTmpcontact.geom.pos = contactGeom.pos; + SharedTmpcontact.geom.normal = contactGeom.normal; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(SharedTmpcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); + } + + 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; + } + + /// + /// 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 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; + /* + // debug + PhysicsActor dp2; + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + { + d.AABB aabb; + d.GeomGetAABB(g2, out aabb); + float x = aabb.MaxX - aabb.MinX; + float y = aabb.MaxY - aabb.MinY; + float z = aabb.MaxZ - aabb.MinZ; + if (x > 60.0f || y > 60.0f || z > 60.0f) + { + if (!actor_name_map.TryGetValue(g2, out dp2)) + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + else + m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", + dp2.Name, dp2.Size, x, y, z, + dp2.Position.ToString(), + dp2.Orientation.ToString(), + dp2.Orientation.Length()); + return; + } + } + // + */ + + + if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || + d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) + { + int cflags; + unchecked + { + cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); + } + count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + else + 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; + } + + // contacts done + if (count == 0) + return; + + // try get physical actors + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); + return; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); + return; + } + + + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + + if (!GetCurContactGeom(0, ref curContact)) + return; + + ContactPoint maxDepthContact = new ContactPoint(); + + // do volume detection case + if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) + { + 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, false + ); + + collision_accounting_events(p1, p2, maxDepthContact); + return; + } + + // big messy collision analises + + float mu = 0; + float bounce = 0; +// bool IgnoreNegSides = false; + + ContactData contactdata1 = new ContactData(0, 0, false); + ContactData contactdata2 = new ContactData(0, 0, false); + + bool dop1ava = false; + bool dop2ava = false; + bool ignore = false; + bool smoothMesh = false; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + { + dop1ava = true; + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + case (int)ActorTypes.Prim: + break; + + default: + ignore = true; // avatar to terrain and water ignored + break; + } + break; + } + + case (int)ActorTypes.Prim: + { + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + dop2ava = true; + break; + + case (int)ActorTypes.Prim: + Vector3 relV = p1.rootVelocity - p2.rootVelocity; + float relVlenSQ = relV.LengthSquared(); + if (relVlenSQ > 0.0001f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + p1.getContactData(ref contactdata1); + p2.getContactData(ref contactdata2); + bounce = contactdata1.bounce * contactdata2.bounce; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if (relVlenSQ > 0.01f) + mu *= frictionMovementMult; + + if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass && + d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + smoothMesh = true; + break; + + case (int)ActorTypes.Ground: + p1.getContactData(ref contactdata1); + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + + Vector3 v1 = p1.rootVelocity; + if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + + if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) + smoothMesh = true; + break; + + case (int)ActorTypes.Water: + default: + ignore = true; + break; + } + } + break; + + case (int)ActorTypes.Ground: + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + p2.CollidingGround = true; + p2.getContactData(ref contactdata2); + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + +// if (curContact.side1 > 0) // should be 2 ? +// IgnoreNegSides = true; + Vector3 v2 = p2.rootVelocity; + if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f) + mu *= frictionMovementMult; + + if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass) + smoothMesh = true; + } + else + ignore = true; + break; + + case (int)ActorTypes.Water: + default: + break; + } + + if (ignore) + return; + + IntPtr Joint; + bool FeetCollision = false; + int ncontacts = 0; + + int i = 0; + + maxDepthContact = new ContactPoint(); + maxDepthContact.PenetrationDepth = float.MinValue; + ContactPoint minDepthContact = new ContactPoint(); + minDepthContact.PenetrationDepth = float.MaxValue; + + SharedTmpcontact.geom.depth = 0; + SharedTmpcontact.surface.mu = mu; + SharedTmpcontact.surface.bounce = bounce; + + d.ContactGeom altContact = new d.ContactGeom(); + bool useAltcontact; + bool noskip; + + if(dop1ava || dop2ava) + smoothMesh = false; + + while (true) + { + noskip = true; + useAltcontact = false; + + if (dop1ava) + { + if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) + { + if (p2.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p2.rootVelocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + } + else + noskip = false; + } + else if (dop2ava) + { + if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) + { + if (p1.PhysicsActorType == (int)ActorTypes.Agent) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + else if (p1.rootVelocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + } + else + noskip = false; + } + + if (noskip) + { + if(useAltcontact) + Joint = CreateContacJoint(ref altContact,smoothMesh); + else + Joint = CreateContacJoint(ref curContact,smoothMesh); + if (Joint == IntPtr.Zero) + break; + + d.JointAttach(Joint, b1, b2); + + ncontacts++; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + maxDepthContact.CharacterFeet = FeetCollision; + } + + if (curContact.depth < minDepthContact.PenetrationDepth) + { + minDepthContact.PenetrationDepth = curContact.depth; + minDepthContact.SurfaceNormal.X = curContact.normal.X; + minDepthContact.SurfaceNormal.Y = curContact.normal.Y; + minDepthContact.SurfaceNormal.Z = curContact.normal.Z; + } + } + + if (++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + } + + if (ncontacts > 0) + { + maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; + maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; + maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; + + collision_accounting_events(p1, p2, maxDepthContact); + } + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + uint obj2LocalID = 0; + + // update actors collision score + if (p1.CollisionScore < float.MaxValue) + p1.CollisionScore += 1.0f; + if (p2.CollisionScore < float.MaxValue) + p2.CollisionScore += 1.0f; + + bool p1events = p1.SubscribedEvents(); + bool p2events = p2.SubscribedEvents(); + + if (p1.IsVolumeDtc) + p2events = false; + if (p2.IsVolumeDtc) + p1events = false; + + if (!p2events && !p1events) + return; + + Vector3 vel = Vector3.Zero; + if (p2 != null && p2.IsPhysical) + vel = p2.rootVelocity; + + if (p1 != null && p1.IsPhysical) + vel -= p1.rootVelocity; + + contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); + + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + case ActorTypes.Prim: + { + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + case ActorTypes.Prim: + if (p2events) + { + //AddCollisionEventReporting(p2); + p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); + } + else if(p1.IsVolumeDtc) + p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact); + + obj2LocalID = p2.ParentActor.LocalID; + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1events) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + contact.RelativeSpeed = -contact.RelativeSpeed; + //AddCollisionEventReporting(p1); + p1.AddCollisionEvent(obj2LocalID, contact); + } + else if(p2.IsVolumeDtc) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + contact.RelativeSpeed = -contact.RelativeSpeed; + //AddCollisionEventReporting(p1); + p1.AddVDTCCollisionEvent(obj2LocalID, contact); + } + break; + } + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + { + if (p2events && !p2.IsVolumeDtc) + { + //AddCollisionEventReporting(p2); + p2.AddCollisionEvent(0, contact); + } + break; + } + } + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized() + { + lock (_characters) + { + try + { + foreach (OdeCharacter chr in _characters) + { + if (chr == null) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + + if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero ) + continue; + + // do colisions with static space + d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); + + // no coll with gnd + } + // chars with chars + d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); + + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); + } + + } + + lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body)) + aprim.clearSleeperCollisions(); + } + } + + lock (_activegroups) + { + try + { + foreach (OdePrim aprim in _activegroups) + { + if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) && + aprim.collide_geom != IntPtr.Zero) + { + d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback); + d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback); + } + } + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message); + } + } + + // colide active amoung them + try + { + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message); + } + + // and with chars + try + { + d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message); + } + } + + #endregion + /// + /// Add actor to the list that should receive collision events in the simulate loop. + /// + /// + public void AddCollisionEventReporting(PhysicsActor obj) + { + 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(_collisionEventPrimRemove) + { + if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) + _collisionEventPrimRemove.Add(obj); + } + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) + { + return null; + } + + public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) + { + OdeCharacter newAv = new OdeCharacter(localID, avName, this, position, + size, feetOffset, 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"); + lock (OdeLock) + { + d.AllocateODEDataForThread(0); + ((OdeCharacter) actor).Destroy(); + } + } + + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + } + } + + public void addActiveGroups(OdePrim activatePrim) + { + lock (_activegroups) + { + if (!_activegroups.Contains(activatePrim)) + _activegroups.Add(activatePrim); + } + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) + { + OdePrim newPrim; + lock (OdeLock) + { + + newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); + } + return newPrim; + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); + } + + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) + { + return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); + } + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + public void remActiveGroup(OdePrim deactivatePrim) + { + lock (_activegroups) + { + _activegroups.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(); + } + } + } + + public void RemovePrimThreadLocked(OdePrim prim) + { + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { +// RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim.LocalID); + } + + } + + public void addToPrims(OdePrim prim) + { + lock (_prims) + _prims[prim.LocalID] = prim; + } + + public OdePrim getPrim(uint id) + { + lock (_prims) + { + if(_prims.ContainsKey(id)) + return _prims[id]; + else + return null; + } + } + + public bool havePrim(OdePrim prm) + { + lock (_prims) + return _prims.ContainsKey(prm.LocalID); + } + + public void changePrimID(OdePrim prim,uint oldID) + { + lock (_prims) + { + if(_prims.ContainsKey(oldID)) + _prims.Remove(oldID); + _prims[prim.LocalID] = prim; + } + } + + public bool haveActor(PhysicsActor actor) + { + if (actor is OdePrim) + { + lock (_prims) + return _prims.ContainsKey(((OdePrim)actor).LocalID); + } + else if (actor is OdeCharacter) + { + lock (_characters) + return _characters.Contains((OdeCharacter)actor); + } + return false; + } + + #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); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + } + 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); + + if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) + { + d.SpaceDestroy(currentspace); + } + + } + } + } + + // 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; + + if (pos.X < 0) + return staticPrimspaceOffRegion[0]; + + if (pos.Y < 0) + return staticPrimspaceOffRegion[2]; + + x = (int)(pos.X * spacesPerMeterX); + if (x > spaceGridMaxX) + return staticPrimspaceOffRegion[1]; + + y = (int)(pos.Y * spacesPerMeterY); + if (y > spaceGridMaxY) + return staticPrimspaceOffRegion[3]; + + return staticPrimspace[x, y]; + } + + #endregion + + + /// + /// 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) + { + } + + // does all pending changes generated during region load process + public override void ProcessPreSimulation() + { + lock (OdeLock) + { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return; + } + + d.AllocateODEDataForThread(~0U); + + ODEchangeitem item; + + int donechanges = 0; + if (ChangesQueue.Count > 0) + { + m_log.InfoFormat("[ubOde] start processing pending actor operations"); + int tstart = Util.EnvironmentTickCount(); + + 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.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + donechanges++; + } + int time = Util.EnvironmentTickCountSubtract(tstart); + m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time); + } + m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count); + } + m_lastframe = Util.GetTimeStamp() + 0.5; + step_time = -0.5f; + } + + /// + /// 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 reqTimeStep) + { + double now = Util.GetTimeStamp(); + double timeStep = now - m_lastframe; + m_lastframe = now; + + // acumulate time so we can reduce error + step_time += (float)timeStep; + + if (step_time < HalfOdeStep) + return 0; + + if (framecount < 0) + framecount = 0; + + framecount++; + +// checkThread(); + int nodeframes = 0; + float fps = 0; + + lock (SimulationLock) + lock(OdeLock) + { + if (world == IntPtr.Zero) + { + ChangesQueue.Clear(); + return 0; + } + + ODEchangeitem item; + +// d.WorldSetQuickStepNumIterations(world, curphysiteractions); + + double loopstartMS = Util.GetTimeStampMS(); + double looptimeMS = 0; + double changestimeMS = 0; + double maxChangestime = (int)(reqTimeStep * 500f); // half the time + double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time + +// double collisionTime = 0; +// double qstepTIme = 0; +// double tmpTime = 0; + + d.AllocateODEDataForThread(~0U); + + if (ChangesQueue.Count > 0) + { + 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.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", + item.actor.Name, item.what.ToString()); + } + } + changestimeMS = Util.GetTimeStampMS() - loopstartMS; + if (changestimeMS > maxChangestime) + break; + } + } + + // do simulation taking at most 150ms total time including changes + while (step_time > HalfOdeStep) + { + try + { + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; + + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + defects.Clear(); + } + } + + // Move other active objects + lock (_activegroups) + { + foreach (OdePrim aprim in _activegroups) + { + aprim.Move(); + } + } + + m_rayCastManager.ProcessQueuedRequests(); + +// tmpTime = Util.GetTimeStampMS(); + collision_optimized(); +// collisionTime += Util.GetTimeStampMS() - tmpTime; + + lock(_collisionEventPrimRemove) + { + foreach (PhysicsActor obj in _collisionEventPrimRemove) + _collisionEventPrim.Remove(obj); + + _collisionEventPrimRemove.Clear(); + } + + List sleepers = new List(); + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.SendCollisions((int)(odetimestepMS)); + break; + + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + if (!pobj.m_outbounds) + { + pobj.SendCollisions((int)(odetimestepMS)); + if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected && + !pobj.m_disabled && !pobj.m_building && + !d.BodyIsEnabled(pobj.Body)) + sleepers.Add(pobj); + } + break; + } + } + + foreach(OdePrim prm in sleepers) + prm.SleeperAddCollisionEvents(); + sleepers.Clear(); + + // do a ode simulation step +// tmpTime = Util.GetTimeStampMS(); + d.WorldQuickStep(world, ODE_STEPSIZE); + d.JointGroupEmpty(contactgroup); +// qstepTIme += Util.GetTimeStampMS() - tmpTime; + + // update managed ideia of physical data and do updates to core + /* + 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 (_activegroups) + { + { + foreach (OdePrim actor in _activegroups) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity(framecount); + } + } + } + } + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + nodeframes++; + + looptimeMS = Util.GetTimeStampMS() - loopstartMS; + if (looptimeMS > maxLoopTime) + break; + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + + _badCharacter.Clear(); + } + } + +// information block for in debug breakpoint only +/* + int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); + + int nactivegeoms = 0; + int nactivespaces = 0; + + int nstaticgeoms = 0; + int nstaticspaces = 0; + IntPtr sp; + + for (int i = 0; i < ntopactivegeoms; i++) + { + sp = d.SpaceGetGeom(ActiveSpace, i); + if (d.GeomIsSpace(sp)) + { + nactivespaces++; + nactivegeoms += d.SpaceGetNumGeoms(sp); + } + else + nactivegeoms++; + } + + for (int i = 0; i < ntopstaticgeoms; i++) + { + sp = d.SpaceGetGeom(StaticSpace, i); + if (d.GeomIsSpace(sp)) + { + nstaticspaces++; + nstaticgeoms += d.SpaceGetNumGeoms(sp); + } + else + nstaticgeoms++; + } + + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + + int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray + int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; +*/ +/* + looptimeMS /= nodeframes; + if(looptimeMS > 0.080) + { + collisionTime /= nodeframes; + qstepTIme /= nodeframes; + } +*/ + // 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); + } + + fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep; + + if(step_time < HalfOdeStep) + m_timeDilation = 1.0f; + else if (step_time > m_SkipFramesAtms) + { + // if we lag too much skip frames + m_timeDilation = 0.0f; + step_time = 0; + m_lastframe = Util.GetTimeStamp(); // skip also the time lost + } + else + { + m_timeDilation = ODE_STEPSIZE / step_time; + if (m_timeDilation > 1) + m_timeDilation = 1; + } + + if (m_timeDilation == 1 && now - m_lastMeshExpire > 30) + { + mesher.ExpireReleaseMeshs(); + m_lastMeshExpire = now; + } + + + } + + return fps; + } + + /// + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + public float GetTerrainHeightAtXY(float x, float y) + { + if (TerrainGeom == IntPtr.Zero) + return 0f; + + if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f; + y += 1f; + + // 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 regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples + int regsize = regsizeX; + + if (m_OSOdeLib) + { + if (x < regsizeX - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsizeX - 2; + dx = 0; + } + if (y < regsizeY - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsizeY - 2; + dy = 0; + } + } + else + { + // 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 + + regsize = regsizeY; + + if (x < regsizeX - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsizeX - 2; + dy = 0; + } + if (y < regsizeY - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsizeY - 2; + dx = 0; + } + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeight; + /* + 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 + } + */ + h0 = ((float)heights[iy]); // 0,0 vertice + + if (dy>dx) + { + iy += regsize; + h2 = (float)heights[iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 + } + else + { + iy++; + h2 = (float)heights[iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 + } + + return h0 + h1 + h2; + } + + public Vector3 GetTerrainNormalAtXY(float x, float y) + { + Vector3 norm = new Vector3(0, 0, 1); + + if (TerrainGeom == IntPtr.Zero) + return norm; + + if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0) + return norm; + + // TerrainHeightField for ODE as offset 1m + x += 1f; + y += 1f; + + // 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 regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples + int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples + int regsize = regsizeX; + + int xstep = 1; + int ystep = regsizeX; + bool firstTri = false; + + if (m_OSOdeLib) + { + if (x < regsizeX - 1) + { + ix = (int)x; + dx = x - (float)ix; + } + else // out world use external height + { + ix = regsizeX - 2; + dx = 0; + } + if (y < regsizeY - 1) + { + iy = (int)y; + dy = y - (float)iy; + } + else + { + iy = regsizeY - 2; + dy = 0; + } + firstTri = dy > dx; + } + + else + { + xstep = regsizeY; + ystep = 1; + regsize = regsizeY; + + // 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 < regsizeX - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsizeX - 2; + dy = 0; + } + if (y < regsizeY - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsizeY - 2; + dx = 0; + } + firstTri = dx > dy; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeight; + + if (firstTri) + { + h1 = ((float)heights[iy]); // 0,0 vertice + iy += ystep; + h0 = (float)heights[iy]; // 0,1 + h2 = (float)heights[iy+xstep]; // 1,1 vertice + norm.X = h0 - h2; + norm.Y = h1 - h0; + } + else + { + h2 = ((float)heights[iy]); // 0,0 vertice + iy += xstep; + h0 = ((float)heights[iy]); // 1,0 vertice + h1 = (float)heights[iy+ystep]; // vertice 1,1 + norm.X = h2 - h0; + norm.Y = h0 - h1; + } + norm.Z = 1; + norm.Normalize(); + return norm; + } + + public override void SetTerrain(float[] heightMap) + { + if (m_OSOdeLib) + OSSetTerrain(heightMap); + else + OriSetTerrain(heightMap); + } + + public void OriSetTerrain(float[] heightMap) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; + + // map is rotated + uint heightmapWidth = regionsizeY + 2; + uint heightmapHeight = regionsizeX + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 10f; + const int wrap = 0; + + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + uint xx; + uint yy; + + uint maxXX = regionsizeX - 1; + uint maxYY = regionsizeY - 1; + // flipping map adding one margin all around so things don't fall in edges + + uint xt = 0; + xx = 0; + + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && xx < maxXX) + xx++; + yy = 0; + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxYY) + yy += regionsizeX; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; + } + xt += heightmapHeightSamples; + } + + lock (OdeLock) + { + d.AllocateODEDataForThread(~0U); + + if (TerrainGeom != IntPtr.Zero) + { + actor_name_map.Remove(TerrainGeom); + d.GeomDestroy(TerrainGeom); + + } + + if (TerrainHeightFieldHeightsHandler.IsAllocated) + TerrainHeightFieldHeightsHandler.Free(); + + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, + heightmapHeight, heightmapWidth , + (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale, + offset, thickness, wrap); + + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + + TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); + + if (TerrainGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(TerrainGeom, 0); + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[TerrainGeom] = pa; + +// geom_name_map[GroundGeom] = "Terrain"; + + d.Quaternion q = new d.Quaternion(); + q.X = 0.5f; + q.Y = 0.5f; + q.Z = 0.5f; + q.W = 0.5f; + + d.GeomSetQuaternion(TerrainGeom, ref q); + d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f); + TerrainHeightFieldHeight = _heightmap; + } + else + TerrainHeightFieldHeightsHandler.Free(); + } + } + + public void OSSetTerrain(float[] heightMap) + { + // assumes 1m size grid and constante size square regions + // needs to know about sims around in future + + float[] _heightmap; + + uint regionsizeX = m_regionWidth; + uint regionsizeY = m_regionHeight; + + uint heightmapWidth = regionsizeX + 2; + uint heightmapHeight = regionsizeY + 2; + + uint heightmapWidthSamples = heightmapWidth + 1; + uint heightmapHeightSamples = heightmapHeight + 1; + + _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; + + + float hfmin = float.MaxValue; +// float hfmax = float.MinValue; + float val; + + + uint maxXX = regionsizeX + 1; + uint maxYY = regionsizeY + 1; + // adding one margin all around so things don't fall in edges + + uint xx; + uint yy = 0; + uint yt = 0; + + for (uint y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxYY) + yy += regionsizeX; + xx = 0; + for (uint x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && x < maxXX) + xx++; + + val = heightMap[yy + xx]; + if (val < 0.0f) + val = 0.0f; // no neg terrain as in chode + _heightmap[yt + x] = val; + + if (hfmin > val) + hfmin = val; +// if (hfmax < val) +// hfmax = val; + } + yt += heightmapWidthSamples; + } + + lock (OdeLock) + { + if (TerrainGeom != IntPtr.Zero) + { + actor_name_map.Remove(TerrainGeom); + d.GeomDestroy(TerrainGeom); + } + + if (TerrainHeightFieldHeightsHandler.IsAllocated) + TerrainHeightFieldHeightsHandler.Free(); + + TerrainHeightFieldHeight = null; + + IntPtr HeightmapData = d.GeomOSTerrainDataCreate(); + + const int wrap = 0; + float thickness = hfmin; + if (thickness < 0) + thickness = 1; + + TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, + thickness, wrap); + +// d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1); + if (TerrainGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land)); + d.GeomSetCollideBits(TerrainGeom, 0); + + PhysicsActor pa = new NullPhysicsActor(); + pa.Name = "Terrain"; + pa.PhysicsActorType = (int)ActorTypes.Ground; + actor_name_map[TerrainGeom] = pa; + +// geom_name_map[GroundGeom] = "Terrain"; + + d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f); + TerrainHeightFieldHeight = _heightmap; + } + else + TerrainHeightFieldHeightsHandler.Free(); + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + } + + public override void Dispose() + { + lock (OdeLock) + { + + if (world == IntPtr.Zero) + return; + + d.AllocateODEDataForThread(~0U); + + if (m_meshWorker != null) + m_meshWorker.Stop(); + + if (m_rayCastManager != null) + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + } + + lock (_prims) + { + ChangesQueue.Clear(); + foreach (OdePrim prm in _prims.Values) + { + prm.DoAChange(changes.Remove, null); + _collisionEventPrim.Remove(prm); + } + _prims.Clear(); + } + + OdeCharacter[] chtorem; + lock (_characters) + { + chtorem = new OdeCharacter[_characters.Count]; + _characters.CopyTo(chtorem); + } + + ChangesQueue.Clear(); + foreach (OdeCharacter ch in chtorem) + ch.DoAChange(changes.Remove, null); + + if (TerrainGeom != IntPtr.Zero) + d.GeomDestroy(TerrainGeom); + TerrainGeom = IntPtr.Zero; + + if (TerrainHeightFieldHeightsHandler.IsAllocated) + TerrainHeightFieldHeightsHandler.Free(); + + TerrainHeightFieldHeight = null; + + if (ContactgeomsArray != IntPtr.Zero) + { + Marshal.FreeHGlobal(ContactgeomsArray); + ContactgeomsArray = IntPtr.Zero; + } + if (GlobalContactsArray != IntPtr.Zero) + { + Marshal.FreeHGlobal(GlobalContactsArray); + GlobalContactsArray = IntPtr.Zero; + } + + d.WorldDestroy(world); + world = IntPtr.Zero; + //d.CloseODE(); + } + } + + private int compareByCollisionsDesc(OdePrim A, OdePrim B) + { + return -A.CollisionScore.CompareTo(B.CollisionScore); + } + + public override Dictionary GetTopColliders() + { + Dictionary topColliders; + List orderedPrims; + lock (_activeprims) + orderedPrims = new List(_activeprims); + + orderedPrims.Sort(compareByCollisionsDesc); + topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore); + + return topColliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + req.filter = RayFilterFlags.AllPrims; + + m_rayCastManager.QueueRequest(req); + } + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null) + { + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.AllPrims; + + m_rayCastManager.QueueRequest(req); + } + } + + + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) + { + List ourresults = new List(); + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = RayFilterFlags.AllPrims; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; + } + } + + public override bool SupportsRaycastWorldFiltered() + { + return true; + } + + public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) + { + object SyncObject = new object(); + List ourresults = new List(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourresults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = filter; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return null; + else + return ourresults; + } + } + + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) + { + if (actor == null) + return new List(); + + 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(); + + List ourResults = null; + object SyncObject = new object(); + + RayCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = actor; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeBoxCallback retMethod = delegate(List results) + { + lock (SyncObject) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + } + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.Normal = size; + req.Origin = position; + req.orientation = orientation; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) + { + List ourResults = null; + object SyncObject = new object(); + + ProbeSphereCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = radius; + req.Origin = position; + req.Count = Count; + req.filter = flags; + + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override List PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) + { + IntPtr geom = IntPtr.Zero;; + + if (actor != null) + { + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + } + + List ourResults = null; + object SyncObject = new object(); + + ProbePlaneCallback retMethod = delegate(List results) + { + ourResults = results; + Monitor.PulseAll(SyncObject); + }; + + ODERayRequest req = new ODERayRequest(); + req.actor = null; + req.callbackMethod = retMethod; + req.length = plane.W; + req.Normal.X = plane.X; + req.Normal.Y = plane.Y; + req.Normal.Z = plane.Z; + req.Count = Count; + req.filter = flags; + + lock (SyncObject) + { + m_rayCastManager.QueueRequest(req); + if (!Monitor.Wait(SyncObject, 500)) + return new List(); + } + + if (ourResults == null) + return new List(); + return ourResults; + } + + public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) + { + Util.FireAndForget( delegate + { + ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); + if(sitAvatar != null) + sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); + }); + return 1; + } + + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs new file mode 100644 index 0000000..214205d --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/ODESitAvatar.cs @@ -0,0 +1,356 @@ +/* + * 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. + */ +// Ubit Umarov 2012 +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenSim.Framework; +using OpenSim.Region.PhysicsModules.SharedBase; +using OdeAPI; +using log4net; +using OpenMetaverse; + +namespace OpenSim.Region.PhysicsModule.ubOde +{ + /// + /// + public class ODESitAvatar + { + private ODEScene m_scene; + private ODERayCastRequestManager m_raymanager; + + public ODESitAvatar(ODEScene pScene, ODERayCastRequestManager raymanager) + { + m_scene = pScene; + m_raymanager = raymanager; + } + + private static Vector3 SitAjust = new Vector3(0, 0, 0.4f); + private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit; + + private void RotAroundZ(float x, float y, ref Quaternion ori) + { + double ang = Math.Atan2(y, x); + ang *= 0.5d; + float s = (float)Math.Sin(ang); + float c = (float)Math.Cos(ang); + + ori.X = 0; + ori.Y = 0; + ori.Z = s; + ori.W = c; + } + + + public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse) + { + if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + + IntPtr geom = ((OdePrim)actor).prim_geom; + + Vector3 geopos = d.GeomGetPositionOMV(geom); + Quaternion geomOri = d.GeomGetQuaternionOMV(geom); + +// Vector3 geopos = actor.Position; +// Quaternion geomOri = actor.Orientation; + + Quaternion geomInvOri = Quaternion.Conjugate(geomOri); + + Quaternion ori = Quaternion.Identity; + + Vector3 rayDir = geopos + offset - avCameraPosition; + + float raylen = rayDir.Length(); + if (raylen < 0.001f) + { + PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity); + return; + } + float t = 1 / raylen; + rayDir.X *= t; + rayDir.Y *= t; + rayDir.Z *= t; + + raylen += 30f; // focal point may be far + List rayResults; + + rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags); + if (rayResults.Count == 0) + { +/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim + d.AABB aabb; + d.GeomGetAABB(geom, out aabb); + offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z); + ori = geomInvOri; + offset *= geomInvOri; + PhysicsSitResponse(1, actor.LocalID, offset, ori); +*/ + PhysicsSitResponse(0, actor.LocalID, offset, ori); + return; + } + + int status = 1; + + offset = rayResults[0].Pos - geopos; + + d.GeomClassID geoclass = d.GeomGetClass(geom); + + if (geoclass == d.GeomClassID.SphereClass) + { + float r = d.GeomSphereGetRadius(geom); + + offset.Normalize(); + offset *= r; + + RotAroundZ(offset.X, offset.Y, ref ori); + + if (r < 0.4f) + { + offset = new Vector3(0, 0, r); + } + else + { + if (offset.Z < 0.4f) + { + t = offset.Z; + float rsq = r * r; + + t = 1.0f / (rsq - t * t); + offset.X *= t; + offset.Y *= t; + offset.Z = 0.4f; + t = rsq - 0.16f; + offset.X *= t; + offset.Y *= t; + } + else if (r > 0.8f && offset.Z > 0.8f * r) + { + status = 3; + avOffset.X = -avOffset.X; + avOffset.Z *= 1.6f; + } + } + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(status, actor.LocalID, offset, ori); + return; + } + + Vector3 norm = rayResults[0].Normal; + + if (norm.Z < -0.4f) + { + PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity); + return; + } + + + float SitNormX = -rayDir.X; + float SitNormY = -rayDir.Y; + + Vector3 pivot = geopos + offset; + + float edgeNormalX = norm.X; + float edgeNormalY = norm.Y; + float edgeDirX = -rayDir.X; + float edgeDirY = -rayDir.Y; + Vector3 edgePos = rayResults[0].Pos; + float edgeDist = float.MaxValue; + + bool foundEdge = false; + + if (norm.Z < 0.5f) + { + float rayDist = 4.0f; + + for (int i = 0; i < 6; i++) + { + pivot.X -= 0.01f * norm.X; + pivot.Y -= 0.01f * norm.Y; + pivot.Z -= 0.01f * norm.Z; + + rayDir.X = -norm.X * norm.Z; + rayDir.Y = -norm.Y * norm.Z; + rayDir.Z = 1.0f - norm.Z * norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + if (Math.Abs(rayResults[0].Normal.Z) < 0.7f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + edgeNormalX = norm.X; + edgeNormalY = norm.Y; + edgeDirX = -rayDir.X; + edgeDirY = -rayDir.Y; + } + else + { + foundEdge = true; + edgePos = rayResults[0].Pos; + break; + } + } + + if (!foundEdge) + { + PhysicsSitResponse(0, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + else if (norm.Z > 0.866f) + { + float toCamBaseX = avCameraPosition.X - pivot.X; + float toCamBaseY = avCameraPosition.Y - pivot.Y; + float toCamX = toCamBaseX; + float toCamY = toCamBaseY; + + for (int j = 0; j < 4; j++) + { + float rayDist = 1.0f; + float curEdgeDist = 0.0f; + + for (int i = 0; i < 3; i++) + { + pivot.Z -= 0.01f; + rayDir.X = toCamX; + rayDir.Y = toCamY; + rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z; + rayDir.Normalize(); + + rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims); + if (rayResults.Count == 0) + break; + + curEdgeDist += rayResults[0].Depth; + + if (rayResults[0].Normal.Z > 0.5f) + { + rayDist -= rayResults[0].Depth; + if (rayDist < 0f) + break; + + pivot = rayResults[0].Pos; + norm = rayResults[0].Normal; + } + else + { + foundEdge = true; + if (curEdgeDist < edgeDist) + { + edgeDist = curEdgeDist; + edgeNormalX = rayResults[0].Normal.X; + edgeNormalY = rayResults[0].Normal.Y; + edgeDirX = rayDir.X; + edgeDirY = rayDir.Y; + edgePos = rayResults[0].Pos; + } + break; + } + } + if (foundEdge && edgeDist < 0.2f) + break; + + pivot = geopos + offset; + + switch (j) + { + case 0: + toCamX = -toCamBaseY; + toCamY = toCamBaseX; + break; + case 1: + toCamX = toCamBaseY; + toCamY = -toCamBaseX; + break; + case 2: + toCamX = -toCamBaseX; + toCamY = -toCamBaseY; + break; + default: + break; + } + } + + if (!foundEdge) + { + avOffset.X = -avOffset.X; + avOffset.Z *= 1.6f; + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset += avOffset * ori; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(3, actor.LocalID, offset, ori); + return; + } + avOffset.X *= 0.5f; + } + + SitNormX = edgeNormalX; + SitNormY = edgeNormalY; + if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0) + { + SitNormX = -SitNormX; + SitNormY = -SitNormY; + } + + RotAroundZ(SitNormX, SitNormY, ref ori); + + offset = edgePos + avOffset * ori; + offset -= geopos; + + ori = geomInvOri * ori; + offset *= geomInvOri; + + PhysicsSitResponse(1, actor.LocalID, offset, ori); + return; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..5baab9f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOde/Properties/AssemblyInfo.cs @@ -0,0 +1,61 @@ +/* + * 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; +using Mono.Addins; + +// 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("OpenSim.Region.PhysicsModule.ubOde")] +[assembly : AssemblyDescription("Ubit Variation of ODE")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("ubOde")] +[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(OpenSim.VersionInfo.AssemblyVersionNumber)] +[assembly: Addin("OpenSim.Region.PhysicsModule.ubOde", OpenSim.VersionInfo.VersionNumber)] +[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs new file mode 100644 index 0000000..ea37301 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/HelperTypes.cs @@ -0,0 +1,340 @@ +/* + * 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.Diagnostics; +using System.Globalization; +using OpenMetaverse; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenSim.Region.PhysicsModule.ubODEMeshing; + +public class Vertex : IComparable +{ + Vector3 vector; + + public float X + { + get { return vector.X; } + set { vector.X = value; } + } + + public float Y + { + get { return vector.Y; } + set { vector.Y = value; } + } + + public float Z + { + get { return vector.Z; } + set { vector.Z = value; } + } + + public Vertex(float x, float y, float z) + { + vector.X = x; + vector.Y = y; + vector.Z = z; + } + + public Vertex normalize() + { + float tlength = vector.Length(); + if (tlength != 0f) + { + float mul = 1.0f / tlength; + return new Vertex(vector.X * mul, vector.Y * mul, vector.Z * mul); + } + else + { + return new Vertex(0f, 0f, 0f); + } + } + + public Vertex cross(Vertex v) + { + return new Vertex(vector.Y * v.Z - vector.Z * v.Y, vector.Z * v.X - vector.X * v.Z, vector.X * v.Y - vector.Y * v.X); + } + + // disable warning: mono compiler moans about overloading + // operators hiding base operator but should not according to C# + // language spec +#pragma warning disable 0108 + public static Vertex operator *(Vertex v, Quaternion q) + { + // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + Vertex v2 = new Vertex(0f, 0f, 0f); + + v2.X = q.W * q.W * v.X + + 2f * q.Y * q.W * v.Z - + 2f * q.Z * q.W * v.Y + + q.X * q.X * v.X + + 2f * q.Y * q.X * v.Y + + 2f * q.Z * q.X * v.Z - + q.Z * q.Z * v.X - + q.Y * q.Y * v.X; + + v2.Y = + 2f * q.X * q.Y * v.X + + q.Y * q.Y * v.Y + + 2f * q.Z * q.Y * v.Z + + 2f * q.W * q.Z * v.X - + q.Z * q.Z * v.Y + + q.W * q.W * v.Y - + 2f * q.X * q.W * v.Z - + q.X * q.X * v.Y; + + v2.Z = + 2f * q.X * q.Z * v.X + + 2f * q.Y * q.Z * v.Y + + q.Z * q.Z * v.Z - + 2f * q.W * q.Y * v.X - + q.Y * q.Y * v.Z + + 2f * q.W * q.X * v.Y - + q.X * q.X * v.Z + + q.W * q.W * v.Z; + + return v2; + } + + public static Vertex operator +(Vertex v1, Vertex v2) + { + return new Vertex(v1.X + v2.X, v1.Y + v2.Y, v1.Z + v2.Z); + } + + public static Vertex operator -(Vertex v1, Vertex v2) + { + return new Vertex(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + } + + public static Vertex operator *(Vertex v1, Vertex v2) + { + return new Vertex(v1.X * v2.X, v1.Y * v2.Y, v1.Z * v2.Z); + } + + public static Vertex operator +(Vertex v1, float am) + { + v1.X += am; + v1.Y += am; + v1.Z += am; + return v1; + } + + public static Vertex operator -(Vertex v1, float am) + { + v1.X -= am; + v1.Y -= am; + v1.Z -= am; + return v1; + } + + public static Vertex operator *(Vertex v1, float am) + { + v1.X *= am; + v1.Y *= am; + v1.Z *= am; + return v1; + } + + public static Vertex operator /(Vertex v1, float am) + { + if (am == 0f) + { + return new Vertex(0f,0f,0f); + } + float mul = 1.0f / am; + v1.X *= mul; + v1.Y *= mul; + v1.Z *= mul; + return v1; + } +#pragma warning restore 0108 + + + public float dot(Vertex v) + { + return X * v.X + Y * v.Y + Z * v.Z; + } + + public Vertex(Vector3 v) + { + vector = v; + } + + public Vertex Clone() + { + return new Vertex(X, Y, Z); + } + + public static Vertex FromAngle(double angle) + { + return new Vertex((float) Math.Cos(angle), (float) Math.Sin(angle), 0.0f); + } + + public float Length() + { + return vector.Length(); + } + + public virtual bool Equals(Vertex v, float tolerance) + { + Vertex diff = this - v; + float d = diff.Length(); + if (d < tolerance) + return true; + + return false; + } + + + public int CompareTo(Vertex other) + { + if (X < other.X) + return -1; + + if (X > other.X) + return 1; + + if (Y < other.Y) + return -1; + + if (Y > other.Y) + return 1; + + if (Z < other.Z) + return -1; + + if (Z > other.Z) + return 1; + + return 0; + } + + public static bool operator >(Vertex me, Vertex other) + { + return me.CompareTo(other) > 0; + } + + public static bool operator <(Vertex me, Vertex other) + { + return me.CompareTo(other) < 0; + } + + public String ToRaw() + { + // Why this stuff with the number formatter? + // Well, the raw format uses the english/US notation of numbers + // where the "," separates groups of 1000 while the "." marks the border between 1 and 10E-1. + // The german notation uses these characters exactly vice versa! + // The Float.ToString() routine is a localized one, giving different results depending on the country + // settings your machine works with. Unusable for a machine readable file format :-( + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.NumberDecimalSeparator = "."; + nfi.NumberDecimalDigits = 6; + + String s1 = X.ToString(nfi) + " " + Y.ToString(nfi) + " " + Z.ToString(nfi); + + return s1; + } +} + +public class Triangle +{ + public Vertex v1; + public Vertex v2; + public Vertex v3; + + public Triangle(Vertex _v1, Vertex _v2, Vertex _v3) + { + v1 = _v1; + v2 = _v2; + v3 = _v3; + } + + public Triangle(float _v1x,float _v1y,float _v1z, + float _v2x,float _v2y,float _v2z, + float _v3x,float _v3y,float _v3z) + { + v1 = new Vertex(_v1x, _v1y, _v1z); + v2 = new Vertex(_v2x, _v2y, _v2z); + v3 = new Vertex(_v3x, _v3y, _v3z); + } + + public override String ToString() + { + NumberFormatInfo nfi = new NumberFormatInfo(); + nfi.CurrencyDecimalDigits = 2; + nfi.CurrencyDecimalSeparator = "."; + + String s1 = "<" + v1.X.ToString(nfi) + "," + v1.Y.ToString(nfi) + "," + v1.Z.ToString(nfi) + ">"; + String s2 = "<" + v2.X.ToString(nfi) + "," + v2.Y.ToString(nfi) + "," + v2.Z.ToString(nfi) + ">"; + String s3 = "<" + v3.X.ToString(nfi) + "," + v3.Y.ToString(nfi) + "," + v3.Z.ToString(nfi) + ">"; + + return s1 + ";" + s2 + ";" + s3; + } + + public Vector3 getNormal() + { + // Vertices + + // Vectors for edges + Vector3 e1; + Vector3 e2; + + e1 = new Vector3(v1.X - v2.X, v1.Y - v2.Y, v1.Z - v2.Z); + e2 = new Vector3(v1.X - v3.X, v1.Y - v3.Y, v1.Z - v3.Z); + + // Cross product for normal + Vector3 n = Vector3.Cross(e1, e2); + + // Length + float l = n.Length(); + + // Normalized "normal" + n = n/l; + + return n; + } + + public void invertNormal() + { + Vertex vt; + vt = v1; + v1 = v2; + v2 = vt; + } + + // Dumps a triangle in the "raw faces" format, blender can import. This is for visualisation and + // debugging purposes + public String ToStringRaw() + { + String output = v1.ToRaw() + " " + v2.ToRaw() + " " + v3.ToRaw(); + return output; + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs new file mode 100644 index 0000000..5d2b1f7 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Mesh.cs @@ -0,0 +1,636 @@ +/* + * 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.IO; +using System.Runtime.InteropServices; +using OpenSim.Region.PhysicsModules.SharedBase; +using PrimMesher; +using OpenMetaverse; +using System.Runtime.Serialization; +using System.Runtime.Serialization.Formatters.Binary; + +namespace OpenSim.Region.PhysicsModule.ubODEMeshing +{ + public class MeshBuildingData + { + 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 Dictionary m_vertices; + public List m_triangles; + public float m_obbXmin; + public float m_obbXmax; + public float m_obbYmin; + public float m_obbYmax; + public float m_obbZmin; + public float m_obbZmax; + public Vector3 m_centroid; + public int m_centroidDiv; + + public MeshBuildingData() + { + vertexcomp vcomp = new vertexcomp(); + m_vertices = new Dictionary(vcomp); + m_triangles = new List(); + m_centroid = Vector3.Zero; + m_centroidDiv = 0; + m_obbXmin = float.MaxValue; + m_obbXmax = float.MinValue; + m_obbYmin = float.MaxValue; + m_obbYmax = float.MinValue; + m_obbZmin = float.MaxValue; + m_obbZmax = float.MinValue; + } + } + + [Serializable()] + public class Mesh : IMesh + { + float[] vertices; + int[] indexes; + Vector3 m_obb; + Vector3 m_obboffset; + [NonSerialized()] + MeshBuildingData m_bdata; + [NonSerialized()] + GCHandle vhandler; + [NonSerialized()] + GCHandle ihandler; + [NonSerialized()] + IntPtr m_verticesPtr = IntPtr.Zero; + [NonSerialized()] + IntPtr m_indicesPtr = IntPtr.Zero; + [NonSerialized()] + int m_vertexCount = 0; + [NonSerialized()] + int m_indexCount = 0; + + public int RefCount { get; set; } + public AMeshKey Key { get; set; } + + public Mesh(bool forbuild) + { + if(forbuild) + m_bdata = new MeshBuildingData(); + m_obb = new Vector3(0.5f, 0.5f, 0.5f); + m_obboffset = Vector3.Zero; + } + + public Mesh Scale(Vector3 scale) + { + if (m_verticesPtr == null || m_indicesPtr == null) + return null; + + Mesh result = new Mesh(false); + + float x = scale.X; + float y = scale.Y; + float z = scale.Z; + + float tmp; + tmp = m_obb.X * x; + if(tmp < 0.0005f) + tmp = 0.0005f; + result.m_obb.X = tmp; + + tmp = m_obb.Y * y; + if(tmp < 0.0005f) + tmp = 0.0005f; + result.m_obb.Y = tmp; + + tmp = m_obb.Z * z; + if(tmp < 0.0005f) + tmp = 0.0005f; + result.m_obb.Z = tmp; + + result.m_obboffset.X = m_obboffset.X * x; + result.m_obboffset.Y = m_obboffset.Y * y; + result.m_obboffset.Z = m_obboffset.Z * z; + + result.vertices = new float[vertices.Length]; + int j = 0; + for (int i = 0; i < m_vertexCount; i++) + { + result.vertices[j] = vertices[j] * x; + j++; + result.vertices[j] = vertices[j] * y; + j++; + result.vertices[j] = vertices[j] * z; + j++; + } + + result.indexes = new int[indexes.Length]; + indexes.CopyTo(result.indexes,0); + + result.pinMemory(); + + return result; + } + + public Mesh Clone() + { + Mesh result = new Mesh(false); + + if (m_bdata != null) + { + result.m_bdata = new MeshBuildingData(); + foreach (Triangle t in m_bdata.m_triangles) + { + result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); + } + result.m_bdata.m_centroid = m_bdata.m_centroid; + result.m_bdata.m_centroidDiv = m_bdata.m_centroidDiv; + result.m_bdata.m_obbXmin = m_bdata.m_obbXmin; + result.m_bdata.m_obbXmax = m_bdata.m_obbXmax; + result.m_bdata.m_obbYmin = m_bdata.m_obbYmin; + result.m_bdata.m_obbYmax = m_bdata.m_obbYmax; + result.m_bdata.m_obbZmin = m_bdata.m_obbZmin; + result.m_bdata.m_obbZmax = m_bdata.m_obbZmax; + } + result.m_obb = m_obb; + result.m_obboffset = m_obboffset; + return result; + } + + public void addVertexLStats(Vertex v) + { + float x = v.X; + float y = v.Y; + float z = v.Z; + + m_bdata.m_centroid.X += x; + m_bdata.m_centroid.Y += y; + m_bdata.m_centroid.Z += z; + m_bdata.m_centroidDiv++; + + if (x > m_bdata.m_obbXmax) + m_bdata.m_obbXmax = x; + if (x < m_bdata.m_obbXmin) + m_bdata.m_obbXmin = x; + + if (y > m_bdata.m_obbYmax) + m_bdata.m_obbYmax = y; + if (y < m_bdata.m_obbYmin) + m_bdata.m_obbYmin = y; + + if (z > m_bdata.m_obbZmax) + m_bdata.m_obbZmax = z; + if (z < m_bdata.m_obbZmin) + m_bdata.m_obbZmin = z; + + } + + public void Add(Triangle triangle) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Add to a pinned Mesh"); + + + triangle.v1.X = (float)Math.Round(triangle.v1.X, 6); + triangle.v1.Y = (float)Math.Round(triangle.v1.Y, 6); + triangle.v1.Z = (float)Math.Round(triangle.v1.Z, 6); + triangle.v2.X = (float)Math.Round(triangle.v2.X, 6); + triangle.v2.Y = (float)Math.Round(triangle.v2.Y, 6); + triangle.v2.Z = (float)Math.Round(triangle.v2.Z, 6); + triangle.v3.X = (float)Math.Round(triangle.v3.X, 6); + triangle.v3.Y = (float)Math.Round(triangle.v3.Y, 6); + triangle.v3.Z = (float)Math.Round(triangle.v3.Z, 6); + + 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_bdata.m_vertices.Count == 0) + { + m_bdata.m_centroidDiv = 0; + m_bdata.m_centroid = Vector3.Zero; + } + + if (!m_bdata.m_vertices.ContainsKey(triangle.v1)) + { + m_bdata.m_vertices[triangle.v1] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v1); + } + if (!m_bdata.m_vertices.ContainsKey(triangle.v2)) + { + m_bdata.m_vertices[triangle.v2] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v2); + } + if (!m_bdata.m_vertices.ContainsKey(triangle.v3)) + { + m_bdata.m_vertices[triangle.v3] = m_bdata.m_vertices.Count; + addVertexLStats(triangle.v3); + } + m_bdata.m_triangles.Add(triangle); + } + + public Vector3 GetCentroid() + { + return m_obboffset; + + } + + public Vector3 GetOBB() + { + return m_obb; +/* + float x, y, z; + if (m_bdata.m_centroidDiv > 0) + { + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; + } + else // ?? + { + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + return new Vector3(x, y, z); +*/ + } + + public int numberVertices() + { + return m_bdata.m_vertices.Count; + } + + public int numberTriangles() + { + return m_bdata.m_triangles.Count; + } + + public List getVertexList() + { + List result = new List(); + foreach (Vertex v in m_bdata.m_vertices.Keys) + { + result.Add(new Vector3(v.X, v.Y, v.Z)); + } + return result; + } + + public float[] getVertexListAsFloat() + { + if (m_bdata.m_vertices == null) + throw new NotSupportedException(); + float[] result = new float[m_bdata.m_vertices.Count * 3]; + foreach (KeyValuePair kvp in m_bdata.m_vertices) + { + Vertex v = kvp.Key; + int i = kvp.Value; + result[3 * i + 0] = v.X; + result[3 * i + 1] = v.Y; + result[3 * i + 2] = v.Z; + } + return result; + } + + public float[] getVertexListAsFloatLocked() + { + return null; + } + + public void getVertexListAsPtrToFloatArray(out IntPtr _vertices, out int vertexStride, out int vertexCount) + { + // A vertex is 3 floats + vertexStride = 3 * sizeof(float); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero && m_bdata != null) + { + vertices = getVertexListAsFloat(); + // Each vertex is 3 elements (floats) + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); + } + _vertices = m_verticesPtr; + vertexCount = m_vertexCount; + } + + public int[] getIndexListAsInt() + { + if (m_bdata.m_triangles == null) + throw new NotSupportedException(); + int[] result = new int[m_bdata.m_triangles.Count * 3]; + for (int i = 0; i < m_bdata.m_triangles.Count; i++) + { + Triangle t = m_bdata.m_triangles[i]; + result[3 * i + 0] = m_bdata.m_vertices[t.v1]; + result[3 * i + 1] = m_bdata.m_vertices[t.v2]; + result[3 * i + 2] = m_bdata.m_vertices[t.v3]; + } + return result; + } + + /// + /// creates a list of index values that defines triangle faces. THIS METHOD FREES ALL NON-PINNED MESH DATA + /// + /// + public int[] getIndexListAsIntLocked() + { + return null; + } + + public void getIndexListAsPtrToIntArray(out IntPtr indices, out int triStride, out int indexCount) + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero && m_bdata != null) + { + indexes = getIndexListAsInt(); + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); + } + // A triangle is 3 ints (indices) + triStride = 3 * sizeof(int); + indices = m_indicesPtr; + indexCount = m_indexCount; + } + + public void releasePinned() + { + if (m_verticesPtr != IntPtr.Zero) + { + vhandler.Free(); + GC.RemoveMemoryPressure(Buffer.ByteLength(vertices)); + vertices = null; + m_verticesPtr = IntPtr.Zero; + } + if (m_indicesPtr != IntPtr.Zero) + { + ihandler.Free(); + GC.RemoveMemoryPressure(Buffer.ByteLength(indexes)); + indexes = null; + m_indicesPtr = IntPtr.Zero; + } + } + + /// + /// frees up the source mesh data to minimize memory - call this method after calling get*Locked() functions + /// + public void releaseSourceMeshData() + { + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + } + } + + public void releaseBuildingMeshData() + { + if (m_bdata != null) + { + m_bdata.m_triangles = null; + m_bdata.m_vertices = null; + m_bdata = null; + } + } + + public void Append(IMesh newMesh) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to Append to a pinned Mesh"); + + if (!(newMesh is Mesh)) + return; + + foreach (Triangle t in ((Mesh)newMesh).m_bdata.m_triangles) + Add(t); + } + + // Do a linear transformation of mesh. + public void TransformLinear(float[,] matrix, float[] offset) + { + if (m_indicesPtr != IntPtr.Zero || m_verticesPtr != IntPtr.Zero) + throw new NotSupportedException("Attempt to TransformLinear a pinned Mesh"); + + foreach (Vertex v in m_bdata.m_vertices.Keys) + { + if (v == null) + continue; + float x, y, z; + x = v.X*matrix[0, 0] + v.Y*matrix[1, 0] + v.Z*matrix[2, 0]; + y = v.X*matrix[0, 1] + v.Y*matrix[1, 1] + v.Z*matrix[2, 1]; + z = v.X*matrix[0, 2] + v.Y*matrix[1, 2] + v.Z*matrix[2, 2]; + v.X = x + offset[0]; + v.Y = y + offset[1]; + v.Z = z + offset[2]; + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + if (m_bdata == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + foreach (Triangle t in m_bdata.m_triangles) + { + String s = t.ToStringRaw(); + sw.WriteLine(s); + } + sw.Close(); + } + + public void TrimExcess() + { + m_bdata.m_triangles.TrimExcess(); + } + + public void pinMemory() + { + m_vertexCount = vertices.Length / 3; + vhandler = GCHandle.Alloc(vertices, GCHandleType.Pinned); + m_verticesPtr = vhandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(vertices)); + + m_indexCount = indexes.Length; + ihandler = GCHandle.Alloc(indexes, GCHandleType.Pinned); + m_indicesPtr = ihandler.AddrOfPinnedObject(); + GC.AddMemoryPressure(Buffer.ByteLength(indexes)); + } + + public void PrepForOde() + { + // If there isn't an unmanaged array allocated yet, do it now + if (m_verticesPtr == IntPtr.Zero) + vertices = getVertexListAsFloat(); + + // If there isn't an unmanaged array allocated yet, do it now + if (m_indicesPtr == IntPtr.Zero) + indexes = getIndexListAsInt(); + + float x, y, z; + + if (m_bdata.m_centroidDiv > 0) + { + m_obboffset = new Vector3(m_bdata.m_centroid.X / m_bdata.m_centroidDiv, m_bdata.m_centroid.Y / m_bdata.m_centroidDiv, m_bdata.m_centroid.Z / m_bdata.m_centroidDiv); + x = (m_bdata.m_obbXmax - m_bdata.m_obbXmin) * 0.5f; + if(x < 0.0005f) + x = 0.0005f; + y = (m_bdata.m_obbYmax - m_bdata.m_obbYmin) * 0.5f; + if(y < 0.0005f) + y = 0.0005f; + z = (m_bdata.m_obbZmax - m_bdata.m_obbZmin) * 0.5f; + if(z < 0.0005f) + z = 0.0005f; + } + + else + { + m_obboffset = Vector3.Zero; + x = 0.5f; + y = 0.5f; + z = 0.5f; + } + + m_obb = new Vector3(x, y, z); + + releaseBuildingMeshData(); + pinMemory(); + } + + public bool ToStream(Stream st) + { + if (m_indicesPtr == IntPtr.Zero || m_verticesPtr == IntPtr.Zero) + return false; + + bool ok = true; + + try + { + using(BinaryWriter bw = new BinaryWriter(st)) + { + bw.Write(m_vertexCount); + bw.Write(m_indexCount); + + for (int i = 0; i < 3 * m_vertexCount; i++) + bw.Write(vertices[i]); + for (int i = 0; i < m_indexCount; i++) + bw.Write(indexes[i]); + bw.Write(m_obb.X); + bw.Write(m_obb.Y); + bw.Write(m_obb.Z); + bw.Write(m_obboffset.X); + bw.Write(m_obboffset.Y); + bw.Write(m_obboffset.Z); + bw.Flush(); + bw.Close(); + } + } + catch + { + ok = false; + } + + return ok; + } + + public static Mesh FromStream(Stream st, AMeshKey key) + { + Mesh mesh = new Mesh(false); + + bool ok = true; + try + { + using(BinaryReader br = new BinaryReader(st)) + { + mesh.m_vertexCount = br.ReadInt32(); + mesh.m_indexCount = br.ReadInt32(); + + int n = 3 * mesh.m_vertexCount; + mesh.vertices = new float[n]; + for (int i = 0; i < n; i++) + mesh.vertices[i] = br.ReadSingle(); + + mesh.indexes = new int[mesh.m_indexCount]; + for (int i = 0; i < mesh.m_indexCount; i++) + mesh.indexes[i] = br.ReadInt32(); + + mesh.m_obb.X = br.ReadSingle(); + mesh.m_obb.Y = br.ReadSingle(); + mesh.m_obb.Z = br.ReadSingle(); + + mesh.m_obboffset.X = br.ReadSingle(); + mesh.m_obboffset.Y = br.ReadSingle(); + mesh.m_obboffset.Z = br.ReadSingle(); + } + } + catch + { + ok = false; + } + + if (ok) + { + mesh.pinMemory(); + + mesh.Key = key; + mesh.RefCount = 1; + + return mesh; + } + + mesh.vertices = null; + mesh.indexes = null; + return null; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs new file mode 100644 index 0000000..032d4ed --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Meshmerizer.cs @@ -0,0 +1,1602 @@ +/* + * 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 OpenSim.Framework; +using OpenSim.Region.Framework.Scenes; +using OpenSim.Region.Framework.Interfaces; +using OpenSim.Region.PhysicsModules.SharedBase; +using OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet; +using OpenMetaverse; +using OpenMetaverse.StructuredData; +using System.Drawing; +using System.Threading; +using System.IO.Compression; +using PrimMesher; +using log4net; +using Nini.Config; +using System.Reflection; +using System.IO; + +using Mono.Addins; + +namespace OpenSim.Region.PhysicsModule.ubODEMeshing +{ + [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "ubODEMeshmerizer")] + public class ubMeshmerizer : IMesher, INonSharedRegionModule + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + // Setting baseDir to a path will enable the dumping of raw files + // raw files can be imported by blender so a visual inspection of the results can be done + + private static string cacheControlFilename = "cntr"; + private bool m_Enabled = false; + + public static object diskLock = new object(); + + public bool doMeshFileCache = true; + public bool doCacheExpire = true; + public string cachePath = "MeshCache"; + public TimeSpan CacheExpire; + +// const string baseDir = "rawFiles"; + private const string baseDir = null; //"rawFiles"; + + private bool useMeshiesPhysicsMesh = true; + private bool doConvexPrims = true; + private bool doConvexSculpts = true; + + private Dictionary m_uniqueMeshes = new Dictionary(); + private Dictionary m_uniqueReleasedMeshes = new Dictionary(); + + #region INonSharedRegionModule + public string Name + { + get { return "ubODEMeshmerizer"; } + } + + public Type ReplaceableInterface + { + get { return null; } + } + + public void Initialise(IConfigSource config) + { + IConfig start_config = config.Configs["Startup"]; + + string mesher = start_config.GetString("meshing", string.Empty); + if (mesher == Name) + { + float fcache = 48.0f; + // float fcache = 0.02f; + + IConfig mesh_config = config.Configs["Mesh"]; + if (mesh_config != null) + { + useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); + doConvexPrims = mesh_config.GetBoolean("ConvexPrims",doConvexPrims); + doConvexSculpts = mesh_config.GetBoolean("ConvexSculpts",doConvexPrims); + doMeshFileCache = mesh_config.GetBoolean("MeshFileCache", doMeshFileCache); + cachePath = mesh_config.GetString("MeshFileCachePath", cachePath); + fcache = mesh_config.GetFloat("MeshFileCacheExpireHours", fcache); + doCacheExpire = mesh_config.GetBoolean("MeshFileCacheDoExpire", doCacheExpire); + + m_Enabled = true; + } + + CacheExpire = TimeSpan.FromHours(fcache); + + if(String.IsNullOrEmpty(cachePath)) + doMeshFileCache = false; + + if(doMeshFileCache) + { + if(!checkCache()) + { + doMeshFileCache = false; + doCacheExpire = false; + } + } + else + doCacheExpire = false; + } + } + + public void Close() + { + } + + public void AddRegion(Scene scene) + { + if (!m_Enabled) + return; + + scene.RegisterModuleInterface(this); + } + + public void RemoveRegion(Scene scene) + { + if (!m_Enabled) + return; + + scene.UnregisterModuleInterface(this); + } + + public void RegionLoaded(Scene scene) + { + if (!m_Enabled) + return; + } + + #endregion + + private void ReportPrimError(string message, string primName, PrimMesh primMesh) + { + m_log.Error(message); + m_log.Error("\nPrim Name: " + primName); + m_log.Error("****** PrimMesh Parameters ******\n" + primMesh.ParamsToDisplayString()); + } + + /// + /// Add a submesh to an existing list of coords and faces. + /// + /// + /// Size of entire object + /// + /// + private void AddSubMesh(OSDMap subMeshData, List coords, List faces) + { + // Console.WriteLine("subMeshMap for {0} - {1}", primName, Util.GetFormattedXml((OSD)subMeshMap)); + + // As per http://wiki.secondlife.com/wiki/Mesh/Mesh_Asset_Format, some Mesh Level + // of Detail Blocks (maps) contain just a NoGeometry key to signal there is no + // geometry for this submesh. + if (subMeshData.ContainsKey("NoGeometry") && ((OSDBoolean)subMeshData["NoGeometry"])) + return; + + OpenMetaverse.Vector3 posMax; + OpenMetaverse.Vector3 posMin; + if (subMeshData.ContainsKey("PositionDomain")) + { + posMax = ((OSDMap)subMeshData["PositionDomain"])["Max"].AsVector3(); + posMin = ((OSDMap)subMeshData["PositionDomain"])["Min"].AsVector3(); + } + else + { + posMax = new Vector3(0.5f, 0.5f, 0.5f); + posMin = new Vector3(-0.5f, -0.5f, -0.5f); + } + + ushort faceIndexOffset = (ushort)coords.Count; + + byte[] posBytes = subMeshData["Position"].AsBinary(); + for (int i = 0; i < posBytes.Length; i += 6) + { + ushort uX = Utils.BytesToUInt16(posBytes, i); + ushort uY = Utils.BytesToUInt16(posBytes, i + 2); + ushort uZ = Utils.BytesToUInt16(posBytes, i + 4); + + Coord c = new Coord( + Utils.UInt16ToFloat(uX, posMin.X, posMax.X), + Utils.UInt16ToFloat(uY, posMin.Y, posMax.Y), + Utils.UInt16ToFloat(uZ, posMin.Z, posMax.Z)); + + coords.Add(c); + } + + byte[] triangleBytes = subMeshData["TriangleList"].AsBinary(); + for (int i = 0; i < triangleBytes.Length; i += 6) + { + ushort v1 = (ushort)(Utils.BytesToUInt16(triangleBytes, i) + faceIndexOffset); + ushort v2 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 2) + faceIndexOffset); + ushort v3 = (ushort)(Utils.BytesToUInt16(triangleBytes, i + 4) + faceIndexOffset); + Face f = new Face(v1, v2, v3); + faces.Add(f); + } + } + + /// + /// Create a physics mesh from data that comes with the prim. The actual data used depends on the prim type. + /// + /// + /// + /// + /// + /// + private Mesh CreateMeshFromPrimMesher(string primName, PrimitiveBaseShape primShape, float lod, bool convex) + { +// m_log.DebugFormat( +// "[MESH]: Creating physics proxy for {0}, shape {1}", +// primName, (OpenMetaverse.SculptType)primShape.SculptType); + + List coords; + List faces; + bool needsConvexProcessing = convex; + + if (primShape.SculptEntry) + { + if (((OpenMetaverse.SculptType)primShape.SculptType) == SculptType.Mesh) + { + if (!useMeshiesPhysicsMesh) + return null; + + if (!GenerateCoordsAndFacesFromPrimMeshData(primName, primShape, out coords, out faces, convex)) + return null; + needsConvexProcessing = false; + } + else + { + if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, lod, out coords, out faces)) + return null; + needsConvexProcessing &= doConvexSculpts; + } + } + else + { + if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, lod, convex, out coords, out faces)) + return null; + needsConvexProcessing &= doConvexPrims; + } + + int numCoords = coords.Count; + int numFaces = faces.Count; + + if(numCoords < 3 || (!needsConvexProcessing && numFaces < 1)) + { + m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName); + return null; + } + + if(needsConvexProcessing) + { + List convexcoords; + List convexfaces; + if(CreateBoundingHull(coords, out convexcoords, out convexfaces) && convexcoords != null && convexfaces != null) + { + coords.Clear(); + coords = convexcoords; + numCoords = coords.Count; + + faces.Clear(); + faces = convexfaces; + numFaces = faces.Count; + } + else + m_log.ErrorFormat("[ubMESH]: failed to create convex for {0} using normal mesh", primName); + } + + Mesh mesh = new Mesh(true); + // Add the corresponding triangles to the mesh + for (int i = 0; i < numFaces; i++) + { + Face f = faces[i]; + mesh.Add(new Triangle(coords[f.v1].X, coords[f.v1].Y, coords[f.v1].Z, + coords[f.v2].X, coords[f.v2].Y, coords[f.v2].Z, + coords[f.v3].X, coords[f.v3].Y, coords[f.v3].Z)); + } + + coords.Clear(); + faces.Clear(); + + if(mesh.numberVertices() < 3 || mesh.numberTriangles() < 1) + { + m_log.ErrorFormat("[MESH]: invalid degenerated mesh for prim {0} ignored", primName); + return null; + } + + primShape.SculptData = Utils.EmptyBytes; + + return mesh; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the mesh data the accompanies a prim. + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimMeshData( + string primName, PrimitiveBaseShape primShape, out List coords, out List faces, bool convex) + { +// m_log.DebugFormat("[MESH]: experimental mesh proxy generation for {0}", primName); + + + // for ubOde we have a diferent mesh use priority + // priority is to use full mesh then decomposition + // SL does the oposite + bool usemesh = false; + + coords = new List(); + faces = new List(); + OSD meshOsd = null; + + if (primShape.SculptData == null || primShape.SculptData.Length <= 0) + { +// m_log.InfoFormat("[MESH]: asset data for {0} is zero length", primName); + return false; + } + + long start = 0; + using (MemoryStream data = new MemoryStream(primShape.SculptData)) + { + try + { + OSD osd = OSDParser.DeserializeLLSDBinary(data); + if (osd is OSDMap) + meshOsd = (OSDMap)osd; + else + { + m_log.WarnFormat("[Mesh}: unable to cast mesh asset to OSDMap prim: {0}",primName); + return false; + } + } + catch (Exception e) + { + m_log.Error("[MESH]: Exception deserializing mesh asset header:" + e.ToString()); + return false; + } + + start = data.Position; + } + + if (meshOsd is OSDMap) + { + OSDMap physicsParms = null; + OSDMap map = (OSDMap)meshOsd; + + if (!convex) + { + if (map.ContainsKey("physics_shape")) + physicsParms = (OSDMap)map["physics_shape"]; // old asset format + else if (map.ContainsKey("physics_mesh")) + physicsParms = (OSDMap)map["physics_mesh"]; // new asset format + + if (physicsParms != null) + usemesh = true; + } + + if(!usemesh && (map.ContainsKey("physics_convex"))) + physicsParms = (OSDMap)map["physics_convex"]; + + if (physicsParms == null) + { + //m_log.WarnFormat("[MESH]: unknown mesh type for prim {0}",primName); + return false; + } + + int physOffset = physicsParms["offset"].AsInteger() + (int)start; + int physSize = physicsParms["size"].AsInteger(); + + if (physOffset < 0 || physSize == 0) + return false; // no mesh data in asset + + OSD decodedMeshOsd = new OSD(); + byte[] meshBytes = new byte[physSize]; + System.Buffer.BlockCopy(primShape.SculptData, physOffset, meshBytes, 0, physSize); + + try + { + using (MemoryStream inMs = new MemoryStream(meshBytes)) + { + using (MemoryStream outMs = new MemoryStream()) + { + using (DeflateStream decompressionStream = new DeflateStream(inMs, CompressionMode.Decompress)) + { + byte[] readBuffer = new byte[2048]; + inMs.Read(readBuffer, 0, 2); // skip first 2 bytes in header + int readLen = 0; + + while ((readLen = decompressionStream.Read(readBuffer, 0, readBuffer.Length)) > 0) + outMs.Write(readBuffer, 0, readLen); + + outMs.Flush(); + outMs.Seek(0, SeekOrigin.Begin); + + byte[] decompressedBuf = outMs.GetBuffer(); + + decodedMeshOsd = OSDParser.DeserializeLLSDBinary(decompressedBuf); + } + } + } + } + catch (Exception e) + { + m_log.Error("[MESH]: exception decoding physical mesh prim " + primName +" : " + e.ToString()); + return false; + } + + if (usemesh) + { + OSDArray decodedMeshOsdArray = null; + + // physics_shape is an array of OSDMaps, one for each submesh + if (decodedMeshOsd is OSDArray) + { +// Console.WriteLine("decodedMeshOsd for {0} - {1}", primName, Util.GetFormattedXml(decodedMeshOsd)); + + decodedMeshOsdArray = (OSDArray)decodedMeshOsd; + foreach (OSD subMeshOsd in decodedMeshOsdArray) + { + if (subMeshOsd is OSDMap) + AddSubMesh(subMeshOsd as OSDMap, coords, faces); + } + } + } + else + { + OSDMap cmap = (OSDMap)decodedMeshOsd; + if (cmap == null) + return false; + + byte[] data; + + List vs = new List(); + PHullResult hullr = new PHullResult(); + float3 f3; + Coord c; + Face f; + Vector3 range; + Vector3 min; + + const float invMaxU16 = 1.0f / 65535f; + int t1; + int t2; + int t3; + int i; + int nverts; + int nindexs; + + if (cmap.ContainsKey("Max")) + range = cmap["Max"].AsVector3(); + else + range = new Vector3(0.5f, 0.5f, 0.5f); + + if (cmap.ContainsKey("Min")) + min = cmap["Min"].AsVector3(); + else + min = new Vector3(-0.5f, -0.5f, -0.5f); + + range = range - min; + range *= invMaxU16; + + if(!convex) + { + // if mesh data not present and not convex then we need convex decomposition data + if (cmap.ContainsKey("HullList") && cmap.ContainsKey("Positions")) + { + List hsizes = new List(); + int totalpoints = 0; + data = cmap["HullList"].AsBinary(); + for (i = 0; i < data.Length; i++) + { + t1 = data[i]; + if (t1 == 0) + t1 = 256; + totalpoints += t1; + hsizes.Add(t1); + } + + data = cmap["Positions"].AsBinary(); + int ptr = 0; + int vertsoffset = 0; + + if (totalpoints == data.Length / 6) // 2 bytes per coord, 3 coords per point + { + foreach (int hullsize in hsizes) + { + for (i = 0; i < hullsize; i++ ) + { + t1 = data[ptr++]; + t1 += data[ptr++] << 8; + t2 = data[ptr++]; + t2 += data[ptr++] << 8; + t3 = data[ptr++]; + t3 += data[ptr++] << 8; + + f3 = new float3((t1 * range.X + min.X), + (t2 * range.Y + min.Y), + (t3 * range.Z + min.Z)); + vs.Add(f3); + } + + if(hullsize <3) + { + vs.Clear(); + continue; + } + + if (hullsize <5) + { + foreach (float3 point in vs) + { + c.X = point.x; + c.Y = point.y; + c.Z = point.z; + coords.Add(c); + } + f = new Face(vertsoffset, vertsoffset + 1, vertsoffset + 2); + faces.Add(f); + + if (hullsize == 4) + { + // not sure about orientation.. + f = new Face(vertsoffset, vertsoffset + 2, vertsoffset + 3); + faces.Add(f); + f = new Face(vertsoffset, vertsoffset + 3, vertsoffset + 1); + faces.Add(f); + f = new Face(vertsoffset + 3, vertsoffset + 2, vertsoffset + 1); + faces.Add(f); + } + vertsoffset += vs.Count; + vs.Clear(); + continue; + } + + List indices; + if (!HullUtils.ComputeHull(vs, out indices)) + { + vs.Clear(); + continue; + } + + nverts = vs.Count; + nindexs = indices.Count; + + if (nindexs % 3 != 0) + { + vs.Clear(); + continue; + } + + for (i = 0; i < nverts; i++) + { + c.X = vs[i].x; + c.Y = vs[i].y; + c.Z = vs[i].z; + coords.Add(c); + } + + for (i = 0; i < nindexs; i += 3) + { + t1 = indices[i]; + if (t1 > nverts) + break; + t2 = indices[i + 1]; + if (t2 > nverts) + break; + t3 = indices[i + 2]; + if (t3 > nverts) + break; + f = new Face(vertsoffset + t1, vertsoffset + t2, vertsoffset + t3); + faces.Add(f); + } + vertsoffset += nverts; + vs.Clear(); + } + } + if (coords.Count > 0 && faces.Count > 0) + return true; + } + else + { + // if neither mesh or decomposition present, warn and use convex + //m_log.WarnFormat("[MESH]: Data for PRIM shape type ( mesh or decomposition) not found for prim {0}",primName); + } + } + vs.Clear(); + + if (cmap.ContainsKey("BoundingVerts")) + { + data = cmap["BoundingVerts"].AsBinary(); + + for (i = 0; i < data.Length; ) + { + t1 = data[i++]; + t1 += data[i++] << 8; + t2 = data[i++]; + t2 += data[i++] << 8; + t3 = data[i++]; + t3 += data[i++] << 8; + + f3 = new float3((t1 * range.X + min.X), + (t2 * range.Y + min.Y), + (t3 * range.Z + min.Z)); + vs.Add(f3); + } + + nverts = vs.Count; + + if (nverts < 3) + { + vs.Clear(); + return false; + } + + if (nverts < 5) + { + foreach (float3 point in vs) + { + c.X = point.x; + c.Y = point.y; + c.Z = point.z; + coords.Add(c); + } + + f = new Face(0, 1, 2); + faces.Add(f); + + if (nverts == 4) + { + f = new Face(0, 2, 3); + faces.Add(f); + f = new Face(0, 3, 1); + faces.Add(f); + f = new Face( 3, 2, 1); + faces.Add(f); + } + vs.Clear(); + return true; + } + + List indices; + if (!HullUtils.ComputeHull(vs, out indices)) + return false; + + nindexs = indices.Count; + + if (nindexs % 3 != 0) + return false; + + for (i = 0; i < nverts; i++) + { + c.X = vs[i].x; + c.Y = vs[i].y; + c.Z = vs[i].z; + coords.Add(c); + } + for (i = 0; i < nindexs; i += 3) + { + t1 = indices[i]; + if (t1 > nverts) + break; + t2 = indices[i + 1]; + if (t2 > nverts) + break; + t3 = indices[i + 2]; + if (t3 > nverts) + break; + f = new Face(t1, t2, t3); + faces.Add(f); + } + vs.Clear(); + if (coords.Count > 0 && faces.Count > 0) + return true; + } + else + return false; + } + } + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the sculpt data the accompanies a prim. + /// + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimSculptData( + string primName, PrimitiveBaseShape primShape, float lod, out List coords, out List faces) + { + coords = new List(); + faces = new List(); + PrimMesher.SculptMesh sculptMesh; + Image idata = null; + + if (primShape.SculptData == null || primShape.SculptData.Length == 0) + return false; + + try + { + OpenMetaverse.Imaging.ManagedImage unusedData; + OpenMetaverse.Imaging.OpenJPEG.DecodeToImage(primShape.SculptData, out unusedData, out idata); + + unusedData = null; + + if (idata == null) + { + // In some cases it seems that the decode can return a null bitmap without throwing + // an exception + m_log.WarnFormat("[PHYSICS]: OpenJPEG decoded sculpt data for {0} to a null bitmap. Ignoring.", primName); + return false; + } + } + catch (DllNotFoundException) + { + m_log.Error("[PHYSICS]: OpenJpeg is not installed correctly on this system. Physics Proxy generation failed. Often times this is because of an old version of GLIBC. You must have version 2.4 or above!"); + return false; + } + catch (IndexOutOfRangeException) + { + m_log.Error("[PHYSICS]: OpenJpeg was unable to decode this. Physics Proxy generation failed"); + return false; + } + catch (Exception ex) + { + m_log.Error("[PHYSICS]: Unable to generate a Sculpty physics proxy. Sculpty texture decode failed: " + ex.Message); + return false; + } + + PrimMesher.SculptMesh.SculptType sculptType; + // remove mirror and invert bits + OpenMetaverse.SculptType pbsSculptType = ((OpenMetaverse.SculptType)(primShape.SculptType & 0x3f)); + switch (pbsSculptType) + { + case OpenMetaverse.SculptType.Cylinder: + sculptType = PrimMesher.SculptMesh.SculptType.cylinder; + break; + case OpenMetaverse.SculptType.Plane: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + case OpenMetaverse.SculptType.Torus: + sculptType = PrimMesher.SculptMesh.SculptType.torus; + break; + case OpenMetaverse.SculptType.Sphere: + sculptType = PrimMesher.SculptMesh.SculptType.sphere; + break; + default: + sculptType = PrimMesher.SculptMesh.SculptType.plane; + break; + } + + bool mirror = ((primShape.SculptType & 128) != 0); + bool invert = ((primShape.SculptType & 64) != 0); + + sculptMesh = new PrimMesher.SculptMesh((Bitmap)idata, sculptType, (int)lod, mirror, invert); + + idata.Dispose(); + +// sculptMesh.DumpRaw(baseDir, primName, "primMesh"); + + coords = sculptMesh.coords; + faces = sculptMesh.faces; + + return true; + } + + /// + /// Generate the co-ords and faces necessary to construct a mesh from the shape data the accompanies a prim. + /// + /// + /// + /// + /// Coords are added to this list by the method. + /// Faces are added to this list by the method. + /// true if coords and faces were successfully generated, false if not + private bool GenerateCoordsAndFacesFromPrimShapeData( + string primName, PrimitiveBaseShape primShape, float lod, bool convex, + out List coords, out List faces) + { + PrimMesh primMesh; + coords = new List(); + faces = new List(); + + float pathShearX = primShape.PathShearX < 128 ? (float)primShape.PathShearX * 0.01f : (float)(primShape.PathShearX - 256) * 0.01f; + float pathShearY = primShape.PathShearY < 128 ? (float)primShape.PathShearY * 0.01f : (float)(primShape.PathShearY - 256) * 0.01f; + float pathBegin = (float)primShape.PathBegin * 2.0e-5f; + float pathEnd = 1.0f - (float)primShape.PathEnd * 2.0e-5f; + float pathScaleX = (float)(primShape.PathScaleX - 100) * 0.01f; + float pathScaleY = (float)(primShape.PathScaleY - 100) * 0.01f; + + float profileBegin = (float)primShape.ProfileBegin * 2.0e-5f; + float profileEnd = 1.0f - (float)primShape.ProfileEnd * 2.0e-5f; + + if (profileBegin < 0.0f) + profileBegin = 0.0f; + + if (profileEnd < 0.02f) + profileEnd = 0.02f; + else if (profileEnd > 1.0f) + profileEnd = 1.0f; + + if (profileBegin >= profileEnd) + profileBegin = profileEnd - 0.02f; + + float profileHollow = (float)primShape.ProfileHollow * 2.0e-5f; + if(convex) + profileHollow = 0.0f; + else if (profileHollow > 0.95f) + profileHollow = 0.95f; + + int sides = 4; + LevelOfDetail iLOD = (LevelOfDetail)lod; + byte profshape = (byte)(primShape.ProfileCurve & 0x07); + + if (profshape == (byte)ProfileShape.EquilateralTriangle + || profshape == (byte)ProfileShape.IsometricTriangle + || profshape == (byte)ProfileShape.RightTriangle) + sides = 3; + else if (profshape == (byte)ProfileShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + } + else if (profshape == (byte)ProfileShape.HalfCircle) + { // half circle, prim is a sphere + switch (iLOD) + { + case LevelOfDetail.High: sides = 24; break; + case LevelOfDetail.Medium: sides = 12; break; + case LevelOfDetail.Low: sides = 6; break; + case LevelOfDetail.VeryLow: sides = 3; break; + default: sides = 24; break; + } + + profileBegin = 0.5f * profileBegin + 0.5f; + profileEnd = 0.5f * profileEnd + 0.5f; + } + + int hollowSides = sides; + if (primShape.HollowShape == HollowShape.Circle) + { + switch (iLOD) + { + case LevelOfDetail.High: hollowSides = 24; break; + case LevelOfDetail.Medium: hollowSides = 12; break; + case LevelOfDetail.Low: hollowSides = 6; break; + case LevelOfDetail.VeryLow: hollowSides = 3; break; + default: hollowSides = 24; break; + } + } + else if (primShape.HollowShape == HollowShape.Square) + hollowSides = 4; + else if (primShape.HollowShape == HollowShape.Triangle) + { + if (profshape == (byte)ProfileShape.HalfCircle) + hollowSides = 6; + else + hollowSides = 3; + } + + primMesh = new PrimMesh(sides, profileBegin, profileEnd, profileHollow, hollowSides); + + if (primMesh.errorMessage != null) + if (primMesh.errorMessage.Length > 0) + m_log.Error("[ERROR] " + primMesh.errorMessage); + + primMesh.topShearX = pathShearX; + primMesh.topShearY = pathShearY; + primMesh.pathCutBegin = pathBegin; + primMesh.pathCutEnd = pathEnd; + + if (primShape.PathCurve == (byte)Extrusion.Straight || primShape.PathCurve == (byte) Extrusion.Flexible) + { + primMesh.twistBegin = (primShape.PathTwistBegin * 18) / 10; + primMesh.twistEnd = (primShape.PathTwist * 18) / 10; + primMesh.taperX = pathScaleX; + primMesh.taperY = pathScaleY; + +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Linear) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeLinear(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + else + { + primMesh.holeSizeX = (200 - primShape.PathScaleX) * 0.01f; + primMesh.holeSizeY = (200 - primShape.PathScaleY) * 0.01f; + primMesh.radius = 0.01f * primShape.PathRadiusOffset; + primMesh.revolutions = 1.0f + 0.015f * primShape.PathRevolutions; + primMesh.skew = 0.01f * primShape.PathSkew; + primMesh.twistBegin = (primShape.PathTwistBegin * 36) / 10; + primMesh.twistEnd = (primShape.PathTwist * 36) / 10; + primMesh.taperX = primShape.PathTaperX * 0.01f; + primMesh.taperY = primShape.PathTaperY * 0.01f; + + if(profshape == (byte)ProfileShape.HalfCircle) + { + if(primMesh.holeSizeY < 0.01f) + primMesh.holeSizeY = 0.01f; + else if(primMesh.holeSizeY > 1.0f) + primMesh.holeSizeY = 1.0f; + } + +#if SPAM + m_log.Debug("****** PrimMesh Parameters (Circular) ******\n" + primMesh.ParamsToDisplayString()); +#endif + try + { + primMesh.ExtrudeCircular(); + } + catch (Exception ex) + { + ReportPrimError("Extrusion failure: exception: " + ex.ToString(), primName, primMesh); + return false; + } + } + +// primMesh.DumpRaw(baseDir, primName, "primMesh"); + + coords = primMesh.coords; + faces = primMesh.faces; + + return true; + } + + public AMeshKey GetMeshUniqueKey(PrimitiveBaseShape primShape, Vector3 size, byte lod, bool convex) + { + AMeshKey key = new AMeshKey(); + Byte[] someBytes; + + key.hashB = 5181; + key.hashC = 5181; + ulong hash = 5381; + + if (primShape.SculptEntry) + { + key.uuid = primShape.SculptTexture; + key.hashC = mdjb2(key.hashC, primShape.SculptType); + key.hashC = mdjb2(key.hashC, primShape.PCode); + } + else + { + hash = mdjb2(hash, primShape.PathCurve); + hash = mdjb2(hash, (byte)primShape.HollowShape); + hash = mdjb2(hash, (byte)primShape.ProfileShape); + hash = mdjb2(hash, primShape.PathBegin); + hash = mdjb2(hash, primShape.PathEnd); + hash = mdjb2(hash, primShape.PathScaleX); + hash = mdjb2(hash, primShape.PathScaleY); + hash = mdjb2(hash, primShape.PathShearX); + key.hashA = hash; + hash = key.hashB; + hash = mdjb2(hash, primShape.PathShearY); + hash = mdjb2(hash, (byte)primShape.PathTwist); + hash = mdjb2(hash, (byte)primShape.PathTwistBegin); + hash = mdjb2(hash, (byte)primShape.PathRadiusOffset); + hash = mdjb2(hash, (byte)primShape.PathTaperX); + hash = mdjb2(hash, (byte)primShape.PathTaperY); + hash = mdjb2(hash, primShape.PathRevolutions); + hash = mdjb2(hash, (byte)primShape.PathSkew); + hash = mdjb2(hash, primShape.ProfileBegin); + hash = mdjb2(hash, primShape.ProfileEnd); + hash = mdjb2(hash, primShape.ProfileHollow); + hash = mdjb2(hash, primShape.PCode); + key.hashB = hash; + } + + hash = key.hashC; + + hash = mdjb2(hash, lod); + + if (size == m_MeshUnitSize) + { + hash = hash << 8; + hash |= 8; + } + else + { + someBytes = size.GetBytes(); + for (int i = 0; i < someBytes.Length; i++) + hash = mdjb2(hash, someBytes[i]); + hash = hash << 8; + } + + if (convex) + hash |= 4; + + if (primShape.SculptEntry) + { + hash |= 1; + if (primShape.SculptType == (byte)SculptType.Mesh) + hash |= 2; + } + + key.hashC = hash; + + return key; + } + + private ulong mdjb2(ulong hash, byte c) + { + return ((hash << 5) + hash) + (ulong)c; + } + + private ulong mdjb2(ulong hash, ushort c) + { + hash = ((hash << 5) + hash) + (ulong)((byte)c); + return ((hash << 5) + hash) + (ulong)(c >> 8); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod) + { + return CreateMesh(primName, primShape, size, lod, false,false,false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical) + { + return CreateMesh(primName, primShape, size, lod, false,false,false); + } + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool shouldCache, bool convex, bool forOde) + { + return CreateMesh(primName, primShape, size, lod, false, false, false); + } + + public IMesh GetMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex) + { + Mesh mesh = null; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + AMeshKey key = GetMeshUniqueKey(primShape, size, (byte)lod, convex); + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + mesh.RefCount = 1; + return mesh; + } + } + } + return null; + } + + private static Vector3 m_MeshUnitSize = new Vector3(1.0f, 1.0f, 1.0f); + + public IMesh CreateMesh(String primName, PrimitiveBaseShape primShape, Vector3 size, float lod, bool isPhysical, bool convex, bool forOde) + { +#if SPAM + m_log.DebugFormat("[MESH]: Creating mesh for {0}", primName); +#endif + + Mesh mesh = null; + + if (size.X < 0.01f) size.X = 0.01f; + if (size.Y < 0.01f) size.Y = 0.01f; + if (size.Z < 0.01f) size.Z = 0.01f; + + // try to find a identical mesh on meshs in use + + AMeshKey key = GetMeshUniqueKey(primShape,size,(byte)lod, convex); + + lock (m_uniqueMeshes) + { + m_uniqueMeshes.TryGetValue(key, out mesh); + + if (mesh != null) + { + mesh.RefCount++; + return mesh; + } + + // try to find a identical mesh on meshs recently released + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(key, out mesh); + if (mesh != null) + { + m_uniqueReleasedMeshes.Remove(key); + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + mesh.RefCount = 1; + return mesh; + } + } + } + + Mesh UnitMesh = null; + AMeshKey unitKey = GetMeshUniqueKey(primShape, m_MeshUnitSize, (byte)lod, convex); + + lock (m_uniqueReleasedMeshes) + { + m_uniqueReleasedMeshes.TryGetValue(unitKey, out UnitMesh); + if (UnitMesh != null) + { + UnitMesh.RefCount = 1; + } + } + + if (UnitMesh == null && primShape.SculptEntry && doMeshFileCache) + UnitMesh = GetFromFileCache(unitKey); + + if (UnitMesh == null) + { + UnitMesh = CreateMeshFromPrimMesher(primName, primShape, lod, convex); + + if (UnitMesh == null) + return null; + + UnitMesh.DumpRaw(baseDir, unitKey.ToString(), "Z"); + + if (forOde) + { + // force pinned mem allocation + UnitMesh.PrepForOde(); + } + else + UnitMesh.TrimExcess(); + + UnitMesh.Key = unitKey; + UnitMesh.RefCount = 1; + + if (doMeshFileCache && primShape.SculptEntry) + StoreToFileCache(unitKey, UnitMesh); + + lock (m_uniqueReleasedMeshes) + { + try + { + m_uniqueReleasedMeshes.Add(unitKey, UnitMesh); + } + catch { } + } + } + + mesh = UnitMesh.Scale(size); + mesh.Key = key; + mesh.RefCount = 1; + lock (m_uniqueMeshes) + { + try + { + m_uniqueMeshes.Add(key, mesh); + } + catch { } + } + + return mesh; + } + + public void ReleaseMesh(IMesh imesh) + { + if (imesh == null) + return; + + Mesh mesh = (Mesh)imesh; + + lock (m_uniqueMeshes) + { + int curRefCount = mesh.RefCount; + curRefCount--; + + if (curRefCount > 0) + { + mesh.RefCount = curRefCount; + return; + } + + mesh.RefCount = 0; + m_uniqueMeshes.Remove(mesh.Key); + lock (m_uniqueReleasedMeshes) + { + try + { + m_uniqueReleasedMeshes.Add(mesh.Key, mesh); + } + catch { } + } + } + } + + public void ExpireReleaseMeshs() + { + if (m_uniqueReleasedMeshes.Count == 0) + return; + + List meshstodelete = new List(); + int refcntr; + + lock (m_uniqueReleasedMeshes) + { + foreach (Mesh m in m_uniqueReleasedMeshes.Values) + { + refcntr = m.RefCount; + refcntr--; + if (refcntr > -6) + m.RefCount = refcntr; + else + meshstodelete.Add(m); + } + + foreach (Mesh m in meshstodelete) + { + m_uniqueReleasedMeshes.Remove(m.Key); + m.releaseBuildingMeshData(); + m.releasePinned(); + } + } + } + + public void FileNames(AMeshKey key, out string dir, out string fullFileName) + { + string id = key.ToString(); + string init = id.Substring(0, 1); + dir = System.IO.Path.Combine(cachePath, init); + fullFileName = System.IO.Path.Combine(dir, id); + } + + public string FullFileName(AMeshKey key) + { + string id = key.ToString(); + string init = id.Substring(0,1); + id = System.IO.Path.Combine(init, id); + id = System.IO.Path.Combine(cachePath, id); + return id; + } + + private Mesh GetFromFileCache(AMeshKey key) + { + Mesh mesh = null; + string filename = FullFileName(key); + bool ok = true; + + lock (diskLock) + { + if (File.Exists(filename)) + { + try + { + using(FileStream stream = File.Open(filename, FileMode.Open, FileAccess.Read, FileShare.Read)) + { +// BinaryFormatter bformatter = new BinaryFormatter(); + mesh = Mesh.FromStream(stream,key); + } + + } + catch (Exception e) + { + ok = false; + m_log.ErrorFormat( + "[MESH CACHE]: Failed to get file {0}. Exception {1} {2}", + filename, e.Message, e.StackTrace); + } + + try + { + if (mesh == null || !ok) + File.Delete(filename); + else + File.SetLastAccessTimeUtc(filename, DateTime.UtcNow); + } + catch + { + } + + } + } + + return mesh; + } + + private void StoreToFileCache(AMeshKey key, Mesh mesh) + { + bool ok = false; + + // Make sure the target cache directory exists + string dir = String.Empty; + string filename = String.Empty; + + FileNames(key, out dir, out filename); + + lock (diskLock) + { + Stream stream = null; + try + { + if (!Directory.Exists(dir)) + { + Directory.CreateDirectory(dir); + } + + stream = File.Open(filename, FileMode.Create); + ok = mesh.ToStream(stream); + } + catch (IOException e) + { + m_log.ErrorFormat( + "[MESH CACHE]: Failed to write file {0}. Exception {1} {2}.", + filename, e.Message, e.StackTrace); + ok = false; + } + finally + { + if(stream != null) + stream.Dispose(); + } + + if (!ok && File.Exists(filename)) + { + try + { + File.Delete(filename); + } + catch (IOException e) + { + m_log.ErrorFormat( + "[MESH CACHE]: Failed to delete file {0}",filename); + } + } + } + } + + public void ExpireFileCache() + { + if (!doCacheExpire) + return; + + string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename); + + lock (diskLock) + { + try + { + if (File.Exists(controlfile)) + { + int ndeleted = 0; + int totalfiles = 0; + int ndirs = 0; + DateTime OlderTime = File.GetLastAccessTimeUtc(controlfile) - CacheExpire; + File.SetLastAccessTimeUtc(controlfile, DateTime.UtcNow); + + foreach (string dir in Directory.GetDirectories(cachePath)) + { + try + { + foreach (string file in Directory.GetFiles(dir)) + { + try + { + if (File.GetLastAccessTimeUtc(file) < OlderTime) + { + File.Delete(file); + ndeleted++; + } + } + catch { } + totalfiles++; + } + } + catch { } + ndirs++; + } + + if (ndeleted == 0) + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, no expires", + totalfiles,ndirs); + else + m_log.InfoFormat("[MESH CACHE]: {0} Files in {1} cache folders, expired {2} files accessed before {3}", + totalfiles,ndirs, ndeleted, OlderTime.ToString()); + } + else + { + m_log.Info("[MESH CACHE]: Expire delayed to next startup"); + FileStream fs = File.Create(controlfile,4096,FileOptions.WriteThrough); + fs.Close(); + } + } + catch { } + } + } + + public bool checkCache() + { + string controlfile = System.IO.Path.Combine(cachePath, cacheControlFilename); + lock (diskLock) + { + try + { + if (!Directory.Exists(cachePath)) + { + Directory.CreateDirectory(cachePath); + Thread.Sleep(100); + FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough); + fs.Close(); + return true; + } + } + catch + { + doMeshFileCache = false; + doCacheExpire = false; + return false; + } + finally {} + + if (File.Exists(controlfile)) + return true; + + try + { + Directory.Delete(cachePath, true); + while(Directory.Exists(cachePath)) + Thread.Sleep(100); + } + catch(Exception e) + { + m_log.Error("[MESH CACHE]: failed to delete old version of the cache: " + e.Message); + doMeshFileCache = false; + doCacheExpire = false; + return false; + } + finally {} + try + { + Directory.CreateDirectory(cachePath); + while(!Directory.Exists(cachePath)) + Thread.Sleep(100); + } + catch(Exception e) + { + m_log.Error("[MESH CACHE]: failed to create new cache folder: " + e.Message); + doMeshFileCache = false; + doCacheExpire = false; + return false; + } + finally {} + + try + { + FileStream fs = File.Create(controlfile, 4096, FileOptions.WriteThrough); + fs.Close(); + } + catch(Exception e) + { + m_log.Error("[MESH CACHE]: failed to create new control file: " + e.Message); + doMeshFileCache = false; + doCacheExpire = false; + return false; + } + finally {} + + return true; + } + } + + public bool CreateBoundingHull(List inputVertices, out List convexcoords, out List newfaces) + { + convexcoords = null; + newfaces = null; + HullDesc desc = new HullDesc(); + HullResult result = new HullResult(); + + int nInputVerts = inputVertices.Count; + int i; + + List vs = new List(nInputVerts); + float3 f3; + + //useless copy + for(i = 0 ; i < nInputVerts; i++) + { + f3 = new float3(inputVertices[i].X, inputVertices[i].Y, inputVertices[i].Z); + vs.Add(f3); + } + + desc.Vertices = vs; + desc.Flags = HullFlag.QF_TRIANGLES; + desc.MaxVertices = 256; + + try + { + HullError ret = HullUtils.CreateConvexHull(desc, ref result); + if (ret != HullError.QE_OK) + return false; + int nverts = result.OutputVertices.Count; + int nindx = result.Indices.Count; + if(nverts < 3 || nindx< 3) + return false; + if(nindx % 3 != 0) + return false; + + convexcoords = new List(nverts); + Coord c; + vs = result.OutputVertices; + + for(i = 0 ; i < nverts; i++) + { + c = new Coord(vs[i].x, vs[i].y, vs[i].z); + convexcoords.Add(c); + } + + newfaces = new List(nindx / 3); + List indxs = result.Indices; + int k, l, m; + Face f; + for(i = 0 ; i < nindx;) + { + k = indxs[i++]; + l = indxs[i++]; + m = indxs[i++]; + if(k > nInputVerts) + continue; + if(l > nInputVerts) + continue; + if(m > nInputVerts) + continue; + f = new Face(k,l,m); + newfaces.Add(f); + } + return true; + } + catch + { + + return false; + } + return false; + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs new file mode 100644 index 0000000..e93175f --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/PrimMesher.cs @@ -0,0 +1,1707 @@ +/* + * Copyright (c) Contributors + * 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.Text; +using System.IO; + +namespace PrimMesher +{ + public struct Quat + { + /// X value + public float X; + /// Y value + public float Y; + /// Z value + public float Z; + /// W value + public float W; + + public Quat(float x, float y, float z, float w) + { + X = x; + Y = y; + Z = z; + W = w; + } + + public Quat(Coord axis, float angle) + { + axis = axis.Normalize(); + + angle *= 0.5f; + float c = (float)Math.Cos(angle); + float s = (float)Math.Sin(angle); + + X = axis.X * s; + Y = axis.Y * s; + Z = axis.Z * s; + W = c; + + Normalize(); + } + + public float Length() + { + return (float)Math.Sqrt(X * X + Y * Y + Z * Z + W * W); + } + + public Quat Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1f / mag; + X *= oomag; + Y *= oomag; + Z *= oomag; + W *= oomag; + } + else + { + X = 0f; + Y = 0f; + Z = 0f; + W = 1f; + } + + return this; + } + + public static Quat operator *(Quat q1, Quat q2) + { + float x = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y; + float y = q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X; + float z = q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W; + float w = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z; + return new Quat(x, y, z, w); + } + + public override string ToString() + { + return "< X: " + this.X.ToString() + ", Y: " + this.Y.ToString() + ", Z: " + this.Z.ToString() + ", W: " + this.W.ToString() + ">"; + } + } + + public struct Coord + { + public float X; + public float Y; + public float Z; + + public Coord(float x, float y, float z) + { + this.X = x; + this.Y = y; + this.Z = z; + } + + public float Length() + { + return (float)Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z); + } + + public Coord Invert() + { + this.X = -this.X; + this.Y = -this.Y; + this.Z = -this.Z; + + return this; + } + + public Coord Normalize() + { + const float MAG_THRESHOLD = 0.0000001f; + float mag = Length(); + + // Catch very small rounding errors when normalizing + if (mag > MAG_THRESHOLD) + { + float oomag = 1.0f / mag; + this.X *= oomag; + this.Y *= oomag; + this.Z *= oomag; + } + else + { + this.X = 0.0f; + this.Y = 0.0f; + this.Z = 0.0f; + } + + return this; + } + + public override string ToString() + { + return this.X.ToString() + " " + this.Y.ToString() + " " + this.Z.ToString(); + } + + public static Coord Cross(Coord c1, Coord c2) + { + return new Coord( + c1.Y * c2.Z - c2.Y * c1.Z, + c1.Z * c2.X - c2.Z * c1.X, + c1.X * c2.Y - c2.X * c1.Y + ); + } + + public static Coord operator +(Coord v, Coord a) + { + return new Coord(v.X + a.X, v.Y + a.Y, v.Z + a.Z); + } + + public static Coord operator *(Coord v, Coord m) + { + return new Coord(v.X * m.X, v.Y * m.Y, v.Z * m.Z); + } + + public static Coord operator *(Coord v, Quat q) + { + // From http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + Coord c2 = new Coord(0.0f, 0.0f, 0.0f); + + c2.X = q.W * q.W * v.X + + 2f * q.Y * q.W * v.Z - + 2f * q.Z * q.W * v.Y + + q.X * q.X * v.X + + 2f * q.Y * q.X * v.Y + + 2f * q.Z * q.X * v.Z - + q.Z * q.Z * v.X - + q.Y * q.Y * v.X; + + c2.Y = + 2f * q.X * q.Y * v.X + + q.Y * q.Y * v.Y + + 2f * q.Z * q.Y * v.Z + + 2f * q.W * q.Z * v.X - + q.Z * q.Z * v.Y + + q.W * q.W * v.Y - + 2f * q.X * q.W * v.Z - + q.X * q.X * v.Y; + + c2.Z = + 2f * q.X * q.Z * v.X + + 2f * q.Y * q.Z * v.Y + + q.Z * q.Z * v.Z - + 2f * q.W * q.Y * v.X - + q.Y * q.Y * v.Z + + 2f * q.W * q.X * v.Y - + q.X * q.X * v.Z + + q.W * q.W * v.Z; + + return c2; + } + } + + public struct Face + { + public int primFace; + + // vertices + public int v1; + public int v2; + public int v3; + + public Face(int v1, int v2, int v3) + { + primFace = 0; + + this.v1 = v1; + this.v2 = v2; + this.v3 = v3; + } + + public Coord SurfaceNormal(List coordList) + { + Coord c1 = coordList[this.v1]; + Coord c2 = coordList[this.v2]; + Coord c3 = coordList[this.v3]; + + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + return Coord.Cross(edge1, edge2).Normalize(); + } + } + + internal struct Angle + { + internal float angle; + internal float X; + internal float Y; + + internal Angle(float angle, float x, float y) + { + this.angle = angle; + this.X = x; + this.Y = y; + } + } + + internal class AngleList + { + private float iX, iY; // intersection point + + private static Angle[] angles3 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles4 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles6 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles12 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private static Angle[] angles24 = + { + new Angle(0.0f, 1.0f, 0.0f), + new Angle(0.041666666666666664f, 0.96592582628906831f, 0.25881904510252074f), + new Angle(0.083333333333333329f, 0.86602540378443871f, 0.5f), + new Angle(0.125f, 0.70710678118654757f, 0.70710678118654746f), + new Angle(0.16666666666666667f, 0.5f, 0.8660254037844386f), + new Angle(0.20833333333333331f, 0.25881904510252096f, 0.9659258262890682f), + new Angle(0.25f, 0.0f, 1.0f), + new Angle(0.29166666666666663f, -0.25881904510252063f, 0.96592582628906831f), + new Angle(0.33333333333333333f, -0.5f, 0.86602540378443871f), + new Angle(0.375f, -0.70710678118654746f, 0.70710678118654757f), + new Angle(0.41666666666666663f, -0.86602540378443849f, 0.5f), + new Angle(0.45833333333333331f, -0.9659258262890682f, 0.25881904510252102f), + new Angle(0.5f, -1.0f, 0.0f), + new Angle(0.54166666666666663f, -0.96592582628906842f, -0.25881904510252035f), + new Angle(0.58333333333333326f, -0.86602540378443882f, -0.5f), + new Angle(0.62499999999999989f, -0.70710678118654791f, -0.70710678118654713f), + new Angle(0.66666666666666667f, -0.5f, -0.86602540378443837f), + new Angle(0.70833333333333326f, -0.25881904510252152f, -0.96592582628906809f), + new Angle(0.75f, 0.0f, -1.0f), + new Angle(0.79166666666666663f, 0.2588190451025203f, -0.96592582628906842f), + new Angle(0.83333333333333326f, 0.5f, -0.86602540378443904f), + new Angle(0.875f, 0.70710678118654735f, -0.70710678118654768f), + new Angle(0.91666666666666663f, 0.86602540378443837f, -0.5f), + new Angle(0.95833333333333326f, 0.96592582628906809f, -0.25881904510252157f), + new Angle(1.0f, 1.0f, 0.0f) + }; + + private Angle interpolatePoints(float newPoint, Angle p1, Angle p2) + { + float m = (newPoint - p1.angle) / (p2.angle - p1.angle); + return new Angle(newPoint, p1.X + m * (p2.X - p1.X), p1.Y + m * (p2.Y - p1.Y)); + } + + private void intersection(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) + { // ref: http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ + double denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1); + double uaNumerator = (x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3); + + if (denom != 0.0) + { + double ua = uaNumerator / denom; + iX = (float)(x1 + ua * (x2 - x1)); + iY = (float)(y1 + ua * (y2 - y1)); + } + } + + internal List angles; + + internal void makeAngles(int sides, float startAngle, float stopAngle, bool hasCut) + { + angles = new List(); + + const double twoPi = System.Math.PI * 2.0; + const float twoPiInv = (float)(1.0d / twoPi); + + if (sides < 1) + throw new Exception("number of sides not greater than zero"); + if (stopAngle <= startAngle) + throw new Exception("stopAngle not greater than startAngle"); + + if ((sides == 3 || sides == 4 || sides == 6 || sides == 12 || sides == 24)) + { + startAngle *= twoPiInv; + stopAngle *= twoPiInv; + + Angle[] sourceAngles; + switch (sides) + { + case 3: + sourceAngles = angles3; + break; + case 4: + sourceAngles = angles4; + break; + case 6: + sourceAngles = angles6; + break; + case 12: + sourceAngles = angles12; + break; + default: + sourceAngles = angles24; + break; + } + + int startAngleIndex = (int)(startAngle * sides); + int endAngleIndex = sourceAngles.Length - 1; + + if (hasCut) + { + if (stopAngle < 1.0f) + endAngleIndex = (int)(stopAngle * sides) + 1; + if (endAngleIndex == startAngleIndex) + endAngleIndex++; + + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex + 1; angleIndex++) + { + angles.Add(sourceAngles[angleIndex]); + } + + if (startAngle > 0.0f) + angles[0] = interpolatePoints(startAngle, angles[0], angles[1]); + + if (stopAngle < 1.0f) + { + int lastAngleIndex = angles.Count - 1; + angles[lastAngleIndex] = interpolatePoints(stopAngle, angles[lastAngleIndex - 1], angles[lastAngleIndex]); + } + } + else + { + for (int angleIndex = startAngleIndex; angleIndex < endAngleIndex; angleIndex++) + angles.Add(sourceAngles[angleIndex]); + } + } + else + { + double stepSize = twoPi / sides; + + int startStep = (int)(startAngle / stepSize); + double angle = stepSize * startStep; + int step = startStep; + double stopAngleTest = stopAngle; + if (stopAngle < twoPi) + { + stopAngleTest = stepSize * ((int)(stopAngle / stepSize) + 1); + if (stopAngleTest < stopAngle) + stopAngleTest += stepSize; + if (stopAngleTest > twoPi) + stopAngleTest = twoPi; + } + + while (angle <= stopAngleTest) + { + Angle newAngle; + newAngle.angle = (float)angle; + newAngle.X = (float)System.Math.Cos(angle); + newAngle.Y = (float)System.Math.Sin(angle); + angles.Add(newAngle); + step += 1; + angle = stepSize * step; + } + + if (startAngle > angles[0].angle) + { + Angle newAngle; + intersection(angles[0].X, angles[0].Y, angles[1].X, angles[1].Y, 0.0f, 0.0f, (float)Math.Cos(startAngle), (float)Math.Sin(startAngle)); + newAngle.angle = startAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[0] = newAngle; + } + + int index = angles.Count - 1; + if (stopAngle < angles[index].angle) + { + Angle newAngle; + intersection(angles[index - 1].X, angles[index - 1].Y, angles[index].X, angles[index].Y, 0.0f, 0.0f, (float)Math.Cos(stopAngle), (float)Math.Sin(stopAngle)); + newAngle.angle = stopAngle; + newAngle.X = iX; + newAngle.Y = iY; + angles[index] = newAngle; + } + } + } + } + + /// + /// generates a profile for extrusion + /// + public class Profile + { + private const float twoPi = 2.0f * (float)Math.PI; + + public string errorMessage = null; + + public List coords; + public List faces; + + // use these for making individual meshes for each prim face + public List outerCoordIndices = null; + public List hollowCoordIndices = null; + + public int numOuterVerts = 0; + public int numHollowVerts = 0; + + public int outerFaceNumber = -1; + public int hollowFaceNumber = -1; + + public int bottomFaceNumber = 0; + public int numPrimFaces = 0; + + public Profile() + { + coords = new List(); + faces = new List(); + } + + public Profile(int sides, float profileStart, float profileEnd, float hollow, int hollowSides, bool hasProfileCut, bool createFaces) + { + const float halfSqr2 = 0.7071067811866f; + + coords = new List(); + faces = new List(); + + List hollowCoords = new List(); + + bool hasHollow = (hollow > 0.0f); + + AngleList angles = new AngleList(); + AngleList hollowAngles = new AngleList(); + + float xScale = 0.5f; + float yScale = 0.5f; + if (sides == 4) // corners of a square are sqrt(2) from center + { + xScale = halfSqr2; + yScale = halfSqr2; + } + + float startAngle = profileStart * twoPi; + float stopAngle = profileEnd * twoPi; + + try { angles.makeAngles(sides, startAngle, stopAngle,hasProfileCut); } + catch (Exception ex) + { + + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + + numOuterVerts = angles.angles.Count; + + Angle angle; + Coord newVert = new Coord(); + + // flag to create as few triangles as possible for 3 or 4 side profile + bool simpleFace = (sides < 5 && !hasHollow && !hasProfileCut); + + if (hasHollow) + { + if (sides == hollowSides) + hollowAngles = angles; + else + { + try { hollowAngles.makeAngles(hollowSides, startAngle, stopAngle, hasProfileCut); } + catch (Exception ex) + { + errorMessage = "makeAngles failed: Exception: " + ex.ToString() + + "\nsides: " + sides.ToString() + " startAngle: " + startAngle.ToString() + " stopAngle: " + stopAngle.ToString(); + + return; + } + + int numHollowAngles = hollowAngles.angles.Count; + for (int i = 0; i < numHollowAngles; i++) + { + angle = hollowAngles.angles[i]; + newVert.X = hollow * xScale * angle.X; + newVert.Y = hollow * yScale * angle.Y; + newVert.Z = 0.0f; + + hollowCoords.Add(newVert); + } + } + numHollowVerts = hollowAngles.angles.Count; + } + else if (!simpleFace) + { + Coord center = new Coord(0.0f, 0.0f, 0.0f); + this.coords.Add(center); + } + + int numAngles = angles.angles.Count; + bool hollowsame = (hasHollow && hollowSides == sides); + + for (int i = 0; i < numAngles; i++) + { + angle = angles.angles[i]; + newVert.X = angle.X * xScale; + newVert.Y = angle.Y * yScale; + newVert.Z = 0.0f; + coords.Add(newVert); + if (hollowsame) + { + newVert.X *= hollow; + newVert.Y *= hollow; + hollowCoords.Add(newVert); + } + } + + if (hasHollow) + { + hollowCoords.Reverse(); + coords.AddRange(hollowCoords); + + if (createFaces) + { + int numTotalVerts = numOuterVerts + numHollowVerts; + + if (numOuterVerts == numHollowVerts) + { + Face newFace = new Face(); + + for (int coordIndex = 0; coordIndex < numOuterVerts - 1; coordIndex++) + { + newFace.v1 = coordIndex; + newFace.v2 = coordIndex + 1; + newFace.v3 = numTotalVerts - coordIndex - 1; + faces.Add(newFace); + + newFace.v1 = coordIndex + 1; + newFace.v2 = numTotalVerts - coordIndex - 2; + newFace.v3 = numTotalVerts - coordIndex - 1; + faces.Add(newFace); + } + if (!hasProfileCut) + { + newFace.v1 = numOuterVerts - 1; + newFace.v2 = 0; + newFace.v3 = numOuterVerts; + faces.Add(newFace); + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - 1; + newFace.v3 = numOuterVerts; + faces.Add(newFace); + } + } + else if (numOuterVerts < numHollowVerts) + { + Face newFace = new Face(); + int j = 0; // j is the index for outer vertices + int i; + int maxJ = numOuterVerts - 1; + float curHollowAngle = 0; + for (i = 0; i < numHollowVerts; i++) // i is the index for inner vertices + { + curHollowAngle = hollowAngles.angles[i].angle; + if (j < maxJ) + { + if (angles.angles[j + 1].angle - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = j; + newFace.v3 = j + 1; + faces.Add(newFace); + j++; + } + } + else + { + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[j].angle + 0.000001f) + break; + } + + newFace.v1 = j; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; + + faces.Add(newFace); + } + + if (!hasProfileCut) + { + if (i == numHollowVerts) + { + newFace.v1 = numTotalVerts - numHollowVerts; + newFace.v2 = maxJ; + newFace.v3 = 0; + + faces.Add(newFace); + } + else + { + if (1.0f - curHollowAngle < curHollowAngle - angles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = numTotalVerts - i - 1; + newFace.v2 = maxJ; + newFace.v3 = 0; + + faces.Add(newFace); + } + + for (; i < numHollowVerts - 1; i++) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - i - 2; + newFace.v3 = numTotalVerts - i - 1; + + faces.Add(newFace); + } + } + + newFace.v1 = 0; + newFace.v2 = numTotalVerts - 1; + newFace.v3 = numTotalVerts - numHollowVerts; + faces.Add(newFace); + } + } + else // numHollowVerts < numOuterVerts + { + Face newFace = new Face(); + int j = 0; // j is the index for inner vertices + int maxJ = numHollowVerts - 1; + for (int i = 0; i < numOuterVerts; i++) + { + if (j < maxJ) + if (hollowAngles.angles[j + 1].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[j].angle + 0.000001f) + { + newFace.v1 = i; + newFace.v2 = numTotalVerts - j - 2; + newFace.v3 = numTotalVerts - j - 1; + + faces.Add(newFace); + j += 1; + } + + newFace.v1 = numTotalVerts - j - 1; + newFace.v2 = i; + newFace.v3 = i + 1; + + faces.Add(newFace); + } + + if (!hasProfileCut) + { + int i = numOuterVerts - 1; + + if (hollowAngles.angles[0].angle - angles.angles[i].angle < angles.angles[i].angle - hollowAngles.angles[maxJ].angle + 0.000001f) + { + newFace.v1 = 0; + newFace.v2 = numTotalVerts - 1; + newFace.v3 = numTotalVerts - maxJ - 1; + + faces.Add(newFace); + } + + newFace.v1 = numTotalVerts - maxJ - 1; + newFace.v2 = i; + newFace.v3 = 0; + + faces.Add(newFace); + } + } + } + + } + + else if (createFaces) + { + if (simpleFace) + { + if (sides == 3) + faces.Add(new Face(0, 1, 2)); + else if (sides == 4) + { + faces.Add(new Face(0, 1, 2)); + faces.Add(new Face(0, 2, 3)); + } + } + else + { + for (int i = 1; i < numAngles ; i++) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = i; + newFace.v3 = i + 1; + faces.Add(newFace); + } + if (!hasProfileCut) + { + Face newFace = new Face(); + newFace.v1 = 0; + newFace.v2 = numAngles; + newFace.v3 = 1; + faces.Add(newFace); + } + } + } + + + hollowCoords = null; + } + + + public Profile Copy() + { + return Copy(true); + } + + public Profile Copy(bool needFaces) + { + Profile copy = new Profile(); + + copy.coords.AddRange(coords); + + if (needFaces) + copy.faces.AddRange(faces); + + copy.numOuterVerts = numOuterVerts; + copy.numHollowVerts = numHollowVerts; + + return copy; + } + + public void AddPos(Coord v) + { + this.AddPos(v.X, v.Y, v.Z); + } + + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + } + + public void AddRot(Quat q) + { + int i; + int numVerts = coords.Count; + + for (i = 0; i < numVerts; i++) + coords[i] *= q; + } + + public void Scale(float x, float y) + { + int i; + int numVerts = coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = coords[i]; + vert.X *= x; + vert.X = (float)Math.Round(vert.X,5); + vert.Y *= y; + vert.Y = (float)Math.Round(vert.Y,5); + coords[i] = vert; + } + + if(x == 0f || y == 0f) + faces = new List(); + } + + /// + /// Changes order of the vertex indices and negates the center vertex normal. Does not alter vertex normals of radial vertices + /// + public void FlipNormals() + { + int numFaces = faces.Count; + if(numFaces == 0) + return; + + int i; + Face tmpFace; + int tmp; + + for (i = 0; i < numFaces; i++) + { + tmpFace = faces[i]; + tmp = tmpFace.v3; + tmpFace.v3 = tmpFace.v1; + tmpFace.v1 = tmp; + faces[i] = tmpFace; + } + } + + public void AddValue2FaceVertexIndices(int num) + { + int numFaces = faces.Count; + if(numFaces == 0) + return; + + Face tmpFace; + + for (int i = 0; i < numFaces; i++) + { + tmpFace = faces[i]; + tmpFace.v1 += num; + tmpFace.v2 += num; + tmpFace.v3 += num; + + faces[i] = tmpFace; + } + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < faces.Count; i++) + { + string s = coords[faces[i].v1].ToString(); + s += " " + coords[faces[i].v2].ToString(); + s += " " + coords[faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } + + public struct PathNode + { + public Coord position; + public Quat rotation; + public float xScale; + public float yScale; + public float percentOfPath; + } + + public enum PathType { Linear = 0, Circular = 1, Flexible = 2 } + + public class Path + { + public List pathNodes = new List(); + + public float twistBegin = 0.0f; + public float twistEnd = 0.0f; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private const float twoPi = 2.0f * (float)Math.PI; + + public void Create(PathType pathType, int steps) + { + if (taperX > .9999f) + taperX = 1.0f; + else if (taperX < -.9999f) + taperX = -1.0f; + if (taperY > .9999f) + taperY = 1.0f; + else if (taperY < -.9999f) + taperY = -1.0f; + + if (pathType == PathType.Linear || pathType == PathType.Flexible) + { + int step = 0; + + float length = pathCutEnd - pathCutBegin; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float start = -0.5f; + float stepSize = length / (float)steps; + float percentOfPathMultiplier = stepSize * 0.999999f; + float xOffset = topShearX * pathCutBegin; + float yOffset = topShearY * pathCutBegin; + float zOffset = start; + float xOffsetStepIncrement = topShearX * length / steps; + float yOffsetStepIncrement = topShearY * length / steps; + + float percentOfPath = pathCutBegin; + zOffset += percentOfPath; + + // sanity checks + + bool done = false; + + while (!done) + { + PathNode newNode = new PathNode(); + + newNode.xScale = 1.0f; + if (taperX > 0.0f) + newNode.xScale -= percentOfPath * taperX; + else if(taperX < 0.0f) + newNode.xScale += (1.0f - percentOfPath) * taperX; + + newNode.yScale = 1.0f; + if (taperY > 0.0f) + newNode.yScale -= percentOfPath * taperY; + else if(taperY < 0.0f) + newNode.yScale += (1.0f - percentOfPath) * taperY; + + float twist = twistBegin + twistTotal * percentOfPath; + + newNode.rotation = new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + newNode.position = new Coord(xOffset, yOffset, zOffset); + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + if (step < steps) + { + step += 1; + percentOfPath += percentOfPathMultiplier; + xOffset += xOffsetStepIncrement; + yOffset += yOffsetStepIncrement; + zOffset += stepSize; + if (percentOfPath > pathCutEnd) + done = true; + } + else done = true; + } + } // end of linear path code + + else // pathType == Circular + { + float twistTotal = twistEnd - twistBegin; + + // if the profile has a lot of twist, add more layers otherwise the layers may overlap + // and the resulting mesh may be quite inaccurate. This method is arbitrary and doesn't + // accurately match the viewer + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + { + if (twistTotalAbs > Math.PI * 1.5f) + steps *= 2; + if (twistTotalAbs > Math.PI * 3.0f) + steps *= 2; + } + + float yPathScale = holeSizeY * 0.5f; + float pathLength = pathCutEnd - pathCutBegin; + float totalSkew = skew * 2.0f * pathLength; + float skewStart = pathCutBegin * 2.0f * skew - skew; + float xOffsetTopShearXFactor = topShearX * (0.25f + 0.5f * (0.5f - holeSizeY)); + float yShearCompensation = 1.0f + Math.Abs(topShearY) * 0.25f; + + // It's not quite clear what pushY (Y top shear) does, but subtracting it from the start and end + // angles appears to approximate it's effects on path cut. Likewise, adding it to the angle used + // to calculate the sine for generating the path radius appears to approximate it's effects there + // too, but there are some subtle differences in the radius which are noticeable as the prim size + // increases and it may affect megaprims quite a bit. The effect of the Y top shear parameter on + // the meshes generated with this technique appear nearly identical in shape to the same prims when + // displayed by the viewer. + + float startAngle = (twoPi * pathCutBegin * revolutions) - topShearY * 0.9f; + float endAngle = (twoPi * pathCutEnd * revolutions) - topShearY * 0.9f; + float stepSize = twoPi / stepsPerRevolution; + + int step = (int)(startAngle / stepSize); + float angle = startAngle; + + bool done = false; + while (!done) // loop through the length of the path and add the layers + { + PathNode newNode = new PathNode(); + + float xProfileScale = (1.0f - Math.Abs(skew)) * holeSizeX; + float yProfileScale = holeSizeY; + + float percentOfPath = angle / (twoPi * revolutions); + float percentOfAngles = (angle - startAngle) / (endAngle - startAngle); + + if (taperX > 0.01f) + xProfileScale *= 1.0f - percentOfPath * taperX; + else if (taperX < -0.01f) + xProfileScale *= 1.0f + (1.0f - percentOfPath) * taperX; + + if (taperY > 0.01f) + yProfileScale *= 1.0f - percentOfPath * taperY; + else if (taperY < -0.01f) + yProfileScale *= 1.0f + (1.0f - percentOfPath) * taperY; + + newNode.xScale = xProfileScale; + newNode.yScale = yProfileScale; + + float radiusScale = 1.0f; + if (radius > 0.001f) + radiusScale = 1.0f - radius * percentOfPath; + else if (radius < 0.001f) + radiusScale = 1.0f + radius * (1.0f - percentOfPath); + + float twist = twistBegin + twistTotal * percentOfPath; + + float xOffset = 0.5f * (skewStart + totalSkew * percentOfAngles); + xOffset += (float)Math.Sin(angle) * xOffsetTopShearXFactor; + + float yOffset = yShearCompensation * (float)Math.Cos(angle) * (0.5f - yPathScale) * radiusScale; + + float zOffset = (float)Math.Sin(angle + topShearY) * (0.5f - yPathScale) * radiusScale; + + newNode.position = new Coord(xOffset, yOffset, zOffset); + + // now orient the rotation of the profile layer relative to it's position on the path + // adding taperY to the angle used to generate the quat appears to approximate the viewer + + newNode.rotation = new Quat(new Coord(1.0f, 0.0f, 0.0f), angle + topShearY); + + // next apply twist rotation to the profile layer + if (twistTotal != 0.0f || twistBegin != 0.0f) + newNode.rotation *= new Quat(new Coord(0.0f, 0.0f, 1.0f), twist); + + newNode.percentOfPath = percentOfPath; + + pathNodes.Add(newNode); + + // calculate terms for next iteration + // calculate the angle for the next iteration of the loop + + if (angle >= endAngle - 0.01) + done = true; + else + { + step += 1; + angle = stepSize * step; + if (angle > endAngle) + angle = endAngle; + } + } + } + } + } + + public class PrimMesh + { + public string errorMessage = ""; + private const float twoPi = 2.0f * (float)Math.PI; + + public List coords; +// public List normals; + public List faces; + + private int sides = 4; + private int hollowSides = 4; + private float profileStart = 0.0f; + private float profileEnd = 1.0f; + private float hollow = 0.0f; + public int twistBegin = 0; + public int twistEnd = 0; + public float topShearX = 0.0f; + public float topShearY = 0.0f; + public float pathCutBegin = 0.0f; + public float pathCutEnd = 1.0f; + public float dimpleBegin = 0.0f; + public float dimpleEnd = 1.0f; + public float skew = 0.0f; + public float holeSizeX = 1.0f; // called pathScaleX in pbs + public float holeSizeY = 0.25f; + public float taperX = 0.0f; + public float taperY = 0.0f; + public float radius = 0.0f; + public float revolutions = 1.0f; + public int stepsPerRevolution = 24; + + private bool hasProfileCut = false; + private bool hasHollow = false; + + public int numPrimFaces = 0; + + /// + /// Human readable string representation of the parameters used to create a mesh. + /// + /// + public string ParamsToDisplayString() + { + string s = ""; + s += "sides..................: " + this.sides.ToString(); + s += "\nhollowSides..........: " + this.hollowSides.ToString(); + s += "\nprofileStart.........: " + this.profileStart.ToString(); + s += "\nprofileEnd...........: " + this.profileEnd.ToString(); + s += "\nhollow...............: " + this.hollow.ToString(); + s += "\ntwistBegin...........: " + this.twistBegin.ToString(); + s += "\ntwistEnd.............: " + this.twistEnd.ToString(); + s += "\ntopShearX............: " + this.topShearX.ToString(); + s += "\ntopShearY............: " + this.topShearY.ToString(); + s += "\npathCutBegin.........: " + this.pathCutBegin.ToString(); + s += "\npathCutEnd...........: " + this.pathCutEnd.ToString(); + s += "\ndimpleBegin..........: " + this.dimpleBegin.ToString(); + s += "\ndimpleEnd............: " + this.dimpleEnd.ToString(); + s += "\nskew.................: " + this.skew.ToString(); + s += "\nholeSizeX............: " + this.holeSizeX.ToString(); + s += "\nholeSizeY............: " + this.holeSizeY.ToString(); + s += "\ntaperX...............: " + this.taperX.ToString(); + s += "\ntaperY...............: " + this.taperY.ToString(); + s += "\nradius...............: " + this.radius.ToString(); + s += "\nrevolutions..........: " + this.revolutions.ToString(); + s += "\nstepsPerRevolution...: " + this.stepsPerRevolution.ToString(); + s += "\nhasProfileCut........: " + this.hasProfileCut.ToString(); + s += "\nhasHollow............: " + this.hasHollow.ToString(); + + return s; + } + + public bool HasProfileCut + { + get { return hasProfileCut; } + set { hasProfileCut = value; } + } + + public bool HasHollow + { + get { return hasHollow; } + } + + + /// + /// Constructs a PrimMesh object and creates the profile for extrusion. + /// + /// + /// + /// + /// + /// + /// + public PrimMesh(int _sides, float _profileStart, float _profileEnd, float _hollow, int _hollowSides) + { + coords = new List(); + faces = new List(); + + sides = _sides; + profileStart = _profileStart; + profileEnd = _profileEnd; + hollow = _hollow; + hollowSides = _hollowSides; + + if (sides < 3) + sides = 3; + if (hollowSides < 3) + hollowSides = 3; + if (profileStart < 0.0f) + profileStart = 0.0f; + if (profileEnd > 1.0f) + profileEnd = 1.0f; + if (profileEnd < 0.02f) + profileEnd = 0.02f; + if (profileStart >= profileEnd) + profileStart = profileEnd - 0.02f; + if (hollow > 0.99f) + hollow = 0.99f; + if (hollow < 0.0f) + hollow = 0.0f; + } + + /// + /// Extrudes a profile along a path. + /// + public void Extrude(PathType pathType) + { + bool needEndFaces = false; + + coords = new List(); + faces = new List(); + + int steps = 1; + + float length = pathCutEnd - pathCutBegin; + + hasProfileCut = this.profileEnd - this.profileStart < 0.9999f; + + hasHollow = (this.hollow > 0.001f); + + float twistBegin = this.twistBegin / 360.0f * twoPi; + float twistEnd = this.twistEnd / 360.0f * twoPi; + float twistTotal = twistEnd - twistBegin; + float twistTotalAbs = Math.Abs(twistTotal); + if (twistTotalAbs > 0.01f) + steps += (int)(twistTotalAbs * 3.66); // dahlia's magic number + + float hollow = this.hollow; + float initialProfileRot = 0.0f; + + if (pathType == PathType.Circular) + { + needEndFaces = false; + if (pathCutBegin != 0.0f || pathCutEnd != 1.0f) + needEndFaces = true; + else if (taperX != 0.0f || taperY != 0.0f) + needEndFaces = true; + else if (skew != 0.0f) + needEndFaces = true; + else if (twistTotal != 0.0f) + needEndFaces = true; + else if (radius != 0.0f) + needEndFaces = true; + } + else needEndFaces = true; + + if (pathType == PathType.Circular) + { + if (sides == 3) + { + initialProfileRot = (float)Math.PI; + if (hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (sides == 4) + { + initialProfileRot = 0.25f * (float)Math.PI; + if (hollowSides != 4) + hollow *= 0.707f; + } + else if (sides > 4) + { + initialProfileRot = (float)Math.PI; + if (hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow /= 0.7f; + } + } + } + else + { + if (sides == 3) + { + if (hollowSides == 4) + { + if (hollow > 0.7f) + hollow = 0.7f; + hollow *= 0.707f; + } + else hollow *= 0.5f; + } + else if (sides == 4) + { + initialProfileRot = 1.25f * (float)Math.PI; + if (hollowSides != 4) + hollow *= 0.707f; + } + else if (sides == 24 && hollowSides == 4) + hollow *= 1.414f; + } + + Profile profile = new Profile(sides, profileStart, profileEnd, hollow, hollowSides, + HasProfileCut,true); + errorMessage = profile.errorMessage; + + numPrimFaces = profile.numPrimFaces; + + if (initialProfileRot != 0.0f) + { + profile.AddRot(new Quat(new Coord(0.0f, 0.0f, 1.0f), initialProfileRot)); + } + + Path path = new Path(); + path.twistBegin = twistBegin; + path.twistEnd = twistEnd; + path.topShearX = topShearX; + path.topShearY = topShearY; + path.pathCutBegin = pathCutBegin; + path.pathCutEnd = pathCutEnd; + path.dimpleBegin = dimpleBegin; + path.dimpleEnd = dimpleEnd; + path.skew = skew; + path.holeSizeX = holeSizeX; + path.holeSizeY = holeSizeY; + path.taperX = taperX; + path.taperY = taperY; + path.radius = radius; + path.revolutions = revolutions; + path.stepsPerRevolution = stepsPerRevolution; + + path.Create(pathType, steps); + + int lastNode = path.pathNodes.Count - 1; + + for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + { + PathNode node = path.pathNodes[nodeIndex]; + Profile newLayer = profile.Copy(); + + newLayer.Scale(node.xScale, node.yScale); + newLayer.AddRot(node.rotation); + newLayer.AddPos(node.position); + + // append this layer + int coordsStart = coords.Count; + coords.AddRange(newLayer.coords); + + if (needEndFaces && nodeIndex == 0 && newLayer.faces.Count > 0) + { + newLayer.AddValue2FaceVertexIndices(coordsStart); + newLayer.FlipNormals(); + faces.AddRange(newLayer.faces); + } + + // fill faces between layers + + List linkfaces = new List(); + int numVerts = newLayer.coords.Count; + Face newFace1 = new Face(); + Face newFace2 = new Face(); + + if (nodeIndex > 0) + { + int startVert = coordsStart; + int endVert = coords.Count; + if (!hasProfileCut) + { + if(numVerts > 5 && !hasHollow) + startVert++; + int i = startVert; + for (int l = 0; l < profile.numOuterVerts - 1; l++) + { + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + linkfaces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + linkfaces.Add(newFace2); + i++; + } + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = startVert; + linkfaces.Add(newFace1); + + newFace2.v1 = startVert; + newFace2.v2 = i - numVerts; + newFace2.v3 = startVert - numVerts; + linkfaces.Add(newFace2); + + if (hasHollow) + { + startVert = ++i; + for (int l = 0; l < profile.numHollowVerts - 1; l++) + { + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = i + 1; + linkfaces.Add(newFace1); + + newFace2.v1 = i + 1; + newFace2.v2 = i - numVerts; + newFace2.v3 = i + 1 - numVerts; + linkfaces.Add(newFace2); + i++; + } + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = startVert; + linkfaces.Add(newFace1); + + newFace2.v1 = startVert; + newFace2.v2 = i - numVerts; + newFace2.v3 = startVert - numVerts; + linkfaces.Add(newFace2); + } + } + else + { + for (int i = startVert; i < endVert; i++) + { + int iNext = i + 1; + if (i == endVert - 1) + iNext = startVert; + + newFace1.v1 = i; + newFace1.v2 = i - numVerts; + newFace1.v3 = iNext; + linkfaces.Add(newFace1); + + newFace2.v1 = iNext; + newFace2.v2 = i - numVerts; + newFace2.v3 = iNext - numVerts; + linkfaces.Add(newFace2); + } + } + } + + if(linkfaces.Count > 0) + faces.AddRange(linkfaces); + + if (needEndFaces && nodeIndex == lastNode && newLayer.faces.Count > 0) + { + newLayer.AddValue2FaceVertexIndices(coordsStart); + faces.AddRange(newLayer.faces); + } + + } // for (int nodeIndex = 0; nodeIndex < path.pathNodes.Count; nodeIndex++) + // more cleanup will be done at Meshmerizer.cs + } + + + /// + /// DEPRICATED - use Extrude(PathType.Linear) instead + /// Extrudes a profile along a straight line path. Used for prim types box, cylinder, and prism. + /// + /// + public void ExtrudeLinear() + { + Extrude(PathType.Linear); + } + + + /// + /// DEPRICATED - use Extrude(PathType.Circular) instead + /// Extrude a profile into a circular path prim mesh. Used for prim types torus, tube, and ring. + /// + /// + public void ExtrudeCircular() + { + Extrude(PathType.Circular); + } + + + private Coord SurfaceNormal(Coord c1, Coord c2, Coord c3) + { + Coord edge1 = new Coord(c2.X - c1.X, c2.Y - c1.Y, c2.Z - c1.Z); + Coord edge2 = new Coord(c3.X - c1.X, c3.Y - c1.Y, c3.Z - c1.Z); + + Coord normal = Coord.Cross(edge1, edge2); + + normal.Normalize(); + + return normal; + } + + private Coord SurfaceNormal(Face face) + { + return SurfaceNormal(this.coords[face.v1], this.coords[face.v2], this.coords[face.v3]); + } + + /// + /// Calculate the surface normal for a face in the list of faces + /// + /// + /// + public Coord SurfaceNormal(int faceIndex) + { + int numFaces = this.faces.Count; + if (faceIndex < 0 || faceIndex >= numFaces) + throw new Exception("faceIndex out of range"); + + return SurfaceNormal(this.faces[faceIndex]); + } + + /// + /// Duplicates a PrimMesh object. All object properties are copied by value, including lists. + /// + /// + public PrimMesh Copy() + { + PrimMesh copy = new PrimMesh(this.sides, this.profileStart, this.profileEnd, this.hollow, this.hollowSides); + copy.twistBegin = this.twistBegin; + copy.twistEnd = this.twistEnd; + copy.topShearX = this.topShearX; + copy.topShearY = this.topShearY; + copy.pathCutBegin = this.pathCutBegin; + copy.pathCutEnd = this.pathCutEnd; + copy.dimpleBegin = this.dimpleBegin; + copy.dimpleEnd = this.dimpleEnd; + copy.skew = this.skew; + copy.holeSizeX = this.holeSizeX; + copy.holeSizeY = this.holeSizeY; + copy.taperX = this.taperX; + copy.taperY = this.taperY; + copy.radius = this.radius; + copy.revolutions = this.revolutions; + copy.stepsPerRevolution = this.stepsPerRevolution; + + copy.numPrimFaces = this.numPrimFaces; + copy.errorMessage = this.errorMessage; + + copy.coords = new List(this.coords); + copy.faces = new List(this.faces); + + return copy; + } + + /// + /// Adds a value to each XYZ vertex coordinate in the mesh + /// + /// + /// + /// + public void AddPos(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + Coord vert; + + for (i = 0; i < numVerts; i++) + { + vert = this.coords[i]; + vert.X += x; + vert.Y += y; + vert.Z += z; + this.coords[i] = vert; + } + } + + /// + /// Rotates the mesh + /// + /// + public void AddRot(Quat q) + { + int i; + int numVerts = this.coords.Count; + + for (i = 0; i < numVerts; i++) + this.coords[i] *= q; + } + +#if VERTEX_INDEXER + public VertexIndexer GetVertexIndexer() + { + return null; + } +#endif + + /// + /// Scales the mesh + /// + /// + /// + /// + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + //Coord vert; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + } + + /// + /// Dumps the mesh to a Blender compatible "Raw" format file + /// + /// + /// + /// + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..6503e35 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using Mono.Addins; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("OpenSim.Region.PhysicsModule.ubODEMeshing")] +[assembly: AssemblyDescription("Mesher for ubODE")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("http://opensimulator.org")] +[assembly: AssemblyProduct("OpenSim")] +[assembly: AssemblyCopyright("OpenSimulator developers")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("4b7e35c2-a9dd-4b10-b778-eb417f4f6884")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +[assembly: AssemblyVersion(OpenSim.VersionInfo.AssemblyVersionNumber)] + +[assembly: Addin("OpenSim.Region.PhysicsModule.ubOdeMeshing", OpenSim.VersionInfo.VersionNumber)] +[assembly: AddinDependency("OpenSim.Region.Framework", OpenSim.VersionInfo.VersionNumber)] diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs new file mode 100644 index 0000000..ebe2523 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMap.cs @@ -0,0 +1,238 @@ +/* + * Copyright (c) Contributors + * 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.Text; + +using System.Drawing; +using System.Drawing.Imaging; + +namespace PrimMesher +{ + public class SculptMap + { + public int width; + public int height; + public byte[] redBytes; + public byte[] greenBytes; + public byte[] blueBytes; + + public SculptMap() + { + } + + public SculptMap(Bitmap bm, int lod) + { + int bmW = bm.Width; + int bmH = bm.Height; + + if (bmW == 0 || bmH == 0) + throw new Exception("SculptMap: bitmap has no data"); + + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + + bool needsScaling = false; + bool smallMap = false; + + width = bmW; + height = bmH; + + while (width * height > numLodPixels * 4) + { + width >>= 1; + height >>= 1; + needsScaling = true; + } + + if (needsScaling) + bm = ScaleImage(bm, width, height); + + if (width * height > numLodPixels) + { + smallMap = false; + width >>= 1; + height >>= 1; + } + else + smallMap = true; + + int numBytes = (width + 1) * (height + 1); + redBytes = new byte[numBytes]; + greenBytes = new byte[numBytes]; + blueBytes = new byte[numBytes]; + + int byteNdx = 0; + Color c; + + try + { + for (int y = 0; y <= height; y++) + { + for (int x = 0; x < width; x++) + { + if (smallMap) + c = bm.GetPixel(x, y < height ? y : y - 1); + else + c = bm.GetPixel(x * 2, y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; + } + + if (smallMap) + c = bm.GetPixel(width - 1, y < height ? y : y - 1); + else + c = bm.GetPixel(width * 2 - 1, y < height ? y * 2 : y * 2 - 1); + + redBytes[byteNdx] = c.R; + greenBytes[byteNdx] = c.G; + blueBytes[byteNdx] = c.B; + + ++byteNdx; + } + } + catch (Exception e) + { + if (needsScaling) + bm.Dispose(); + throw new Exception("Caught exception processing byte arrays in SculptMap(): e: " + e.ToString()); + } + + width++; + height++; + if(needsScaling) + bm.Dispose(); + } + + public List> ToRows(bool mirror) + { + int numRows = height; + int numCols = width; + + List> rows = new List>(numRows); + + float pixScale = 1.0f / 255; + + 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(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); + else + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); + + ++smNdx; + } + rows.Add(row); + } + return rows; + } + + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) + { + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + + // will let last step to be eventually diferent, as seems to be in sl + + float xscale = (float)srcImage.Width / (float)destWidth; + float yscale = (float)srcImage.Height / (float)destHeight; + + int lastsx = srcImage.Width - 1; + int lastsy = srcImage.Height - 1; + int lastdx = destWidth - 1; + int lastdy = destHeight - 1; + + float sy = 0.5f; + float sx; + + for (int y = 0; y < lastdy; y++) + { + sx = 0.5f; + for (int x = 0; x < lastdx; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + sx += xscale; + } + try + { + c = srcImage.GetPixel(lastsx, (int)(sy)); + scaledImage.SetPixel(lastdx, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sy += yscale; + } + + sx = 0.5f; + for (int x = 0; x < lastdx; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), lastsy); + scaledImage.SetPixel(x, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + sx += xscale; + } + try + { + c = srcImage.GetPixel(lastsx, lastsy); + scaledImage.SetPixel(lastdx, lastdy, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } + + srcImage.Dispose(); + return scaledImage; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs new file mode 100644 index 0000000..bc1375b --- /dev/null +++ b/OpenSim/Region/PhysicsModules/ubOdeMeshing/SculptMesh.cs @@ -0,0 +1,220 @@ +/* + * Copyright (c) Contributors + * 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.Text; +using System.IO; + +using System.Drawing; +using System.Drawing.Imaging; + +namespace PrimMesher +{ + + public class SculptMesh + { + public List coords; + public List faces; + + public enum SculptType { sphere = 1, torus = 2, plane = 3, cylinder = 4 }; + + + public SculptMesh(Bitmap sculptBitmap, SculptType sculptType, int lod, bool mirror, bool invert) + { + if (mirror) + invert = !invert; + + SculptMap smap = new SculptMap(sculptBitmap, lod); + + List> rows = smap.ToRows(mirror); + + _SculptMesh(rows, sculptType, invert); + } + + private void _SculptMesh(List> rows, SculptType sculptType, bool invert) + { + coords = new List(); + faces = new List(); + + sculptType = (SculptType)(((int)sculptType) & 0x07); + + int width = rows[0].Count; + + int p1, p2, p3, p4; + + int imageX, imageY; + + if (sculptType != SculptType.plane) + { + if (rows.Count % 2 == 0) + { + for (int rowNdx = 0; rowNdx < rows.Count; rowNdx++) + rows[rowNdx].Add(rows[rowNdx][0]); + } + else + { + int lastIndex = rows[0].Count - 1; + + for (int i = 0; i < rows.Count; i++) + rows[i][0] = rows[i][lastIndex]; + } + } + + Coord topPole = rows[0][width / 2]; + Coord bottomPole = rows[rows.Count - 1][width / 2]; + + if (sculptType == SculptType.sphere) + { + if (rows.Count % 2 == 0) + { + int count = rows[0].Count; + List topPoleRow = new List(count); + List bottomPoleRow = new List(count); + + for (int i = 0; i < count; i++) + { + topPoleRow.Add(topPole); + bottomPoleRow.Add(bottomPole); + } + rows.Insert(0, topPoleRow); + rows.Add(bottomPoleRow); + } + else + { + int count = rows[0].Count; + + List topPoleRow = rows[0]; + List bottomPoleRow = rows[rows.Count - 1]; + + for (int i = 0; i < count; i++) + { + topPoleRow[i] = topPole; + bottomPoleRow[i] = bottomPole; + } + } + } + + if (sculptType == SculptType.torus) + rows.Add(rows[0]); + + int coordsDown = rows.Count; + int coordsAcross = rows[0].Count; + + float widthUnit = 1.0f / (coordsAcross - 1); + float heightUnit = 1.0f / (coordsDown - 1); + + for (imageY = 0; imageY < coordsDown; imageY++) + { + int rowOffset = imageY * coordsAcross; + + for (imageX = 0; imageX < coordsAcross; imageX++) + { + /* + * p1-----p2 + * | \ f2 | + * | \ | + * | f1 \| + * p3-----p4 + */ + + p4 = rowOffset + imageX; + p3 = p4 - 1; + + p2 = p4 - coordsAcross; + p1 = p3 - coordsAcross; + + this.coords.Add(rows[imageY][imageX]); + + if (imageY > 0 && imageX > 0) + { + Face f1, f2; + + if (invert) + { + f1 = new Face(p1, p4, p3); + f2 = new Face(p1, p2, p4); + } + else + { + f1 = new Face(p1, p3, p4); + f2 = new Face(p1, p4, p2); + } + + this.faces.Add(f1); + this.faces.Add(f2); + } + } + } + } + + /// + /// Duplicates a SculptMesh object. All object properties are copied by value, including lists. + /// + /// + public SculptMesh Copy() + { + return new SculptMesh(this); + } + + public SculptMesh(SculptMesh sm) + { + coords = new List(sm.coords); + faces = new List(sm.faces); + } + + public void Scale(float x, float y, float z) + { + int i; + int numVerts = this.coords.Count; + + Coord m = new Coord(x, y, z); + for (i = 0; i < numVerts; i++) + this.coords[i] *= m; + } + + public void DumpRaw(String path, String name, String title) + { + if (path == null) + return; + String fileName = name + "_" + title + ".raw"; + String completePath = System.IO.Path.Combine(path, fileName); + StreamWriter sw = new StreamWriter(completePath); + + for (int i = 0; i < this.faces.Count; i++) + { + string s = this.coords[this.faces[i].v1].ToString(); + s += " " + this.coords[this.faces[i].v2].ToString(); + s += " " + this.coords[this.faces[i].v3].ToString(); + + sw.WriteLine(s); + } + + sw.Close(); + } + } +} -- cgit v1.1