From 815f3af1d7b3bf16e81dd3a03e0c69c8e49f2f91 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 15:24:10 +0000 Subject: UbitODE plugin initial commit --- .../Region/Physics/UbitOdePlugin/AssemblyInfo.cs | 58 + .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 1301 ++++++++ .../Physics/UbitOdePlugin/ODEDynamics.c_comments | 630 ++++ .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 1035 +++++++ OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3260 ++++++++++++++++++++ .../UbitOdePlugin/ODERayCastRequestManager.cs | 443 +++ OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 1903 ++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs | 86 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 2741 ++++++++++++++++ OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs | 99 + 10 files changed, 11556 insertions(+) create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs create mode 100644 OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs new file mode 100644 index 0000000..d46341b --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/AssemblyInfo.cs @@ -0,0 +1,58 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System.Reflection; +using System.Runtime.InteropServices; + +// Information about this assembly is defined by the following +// attributes. +// +// change them to the information which is associated with the assembly +// you compile. + +[assembly : AssemblyTitle("OdePlugin")] +[assembly : AssemblyDescription("Ubit Variation")] +[assembly : AssemblyConfiguration("")] +[assembly : AssemblyCompany("http://opensimulator.org")] +[assembly : AssemblyProduct("OdePlugin")] +[assembly : AssemblyCopyright("Copyright (c) OpenSimulator.org Developers 2007-2009")] +[assembly : AssemblyTrademark("")] +[assembly : AssemblyCulture("")] + +// This sets the default COM visibility of types in the assembly to invisible. +// If you need to expose a type to COM, use [ComVisible(true)] on that type. + +[assembly : ComVisible(false)] + +// The assembly version has following format : +// +// Major.Minor.Build.Revision +// +// You can specify all values by your own or you can build default build and revision +// numbers with the '*' character (the default): + +[assembly : AssemblyVersion("0.6.5.*")] diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs new file mode 100644 index 0000000..c8f7c76 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -0,0 +1,1301 @@ +/* + * 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 + +using System; +using System.Collections.Generic; +using System.Reflection; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. + /// + + public enum dParam : int + { + LowStop = 0, + HiStop = 1, + Vel = 2, + FMax = 3, + FudgeFactor = 4, + Bounce = 5, + CFM = 6, + StopERP = 7, + StopCFM = 8, + LoStop2 = 256, + HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, + StopERP2 = 7 + 256, + StopCFM2 = 8 + 256, + LoStop3 = 512, + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515, + StopERP3 = 7 + 512, + StopCFM3 = 8 + 512 + } + + public class OdeCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private Vector3 _position; + private Vector3 _zeroPosition; + private bool _zeroFlag = false; + private Vector3 _velocity; + private Vector3 _target_velocity; + private Vector3 _acceleration; + private Vector3 m_rotationalVelocity; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_pidControllerActive = true; + public float PID_D = 800.0f; + public float PID_P = 900.0f; + //private static float POSTURE_SERVO = 10000.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private bool flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private int m_requestedUpdateFrequency = 0; + private Vector3 m_taintPosition = Vector3.Zero; + private bool m_hasTaintPosition = false; + public uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) + public float MinimumGroundFlightOffset = 3f; + + + private Vector3 m_taintForce; + private bool m_hasTaintForce; + private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. + + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + // other filter control + int m_colliderfilter = 0; + // int m_colliderGroundfilter = 0; + int m_colliderObjectfilter = 0; + + // Default we're a Character + private CollisionCategories m_collisionCategories = (CollisionCategories.Character); + + // Default, Collide with Other Geometries, spaces, bodies and characters. + private CollisionCategories m_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + // we do land collisions not ode | CollisionCategories.Land); + public IntPtr Body = IntPtr.Zero; + private OdeScene _parent_scene; + public IntPtr Shell = IntPtr.Zero; + public IntPtr Amotor = IntPtr.Zero; + public d.Mass ShellMass; + public bool collidelock = false; + + private bool m_haseventsubscription = false; + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + // unique UUID of this character object + public UUID m_uuid; + public bool bad = false; + + public ContactData AvatarContactData = new ContactData(10f, 0.3f); + + public OdeCharacter(String avName, OdeScene parent_scene, Vector3 pos, Vector3 size, float pid_d, float pid_p, float capsule_radius, float density, float walk_divisor, float rundivisor) + { + m_uuid = UUID.Random(); + + m_hasTaintPosition = false; + + if (pos.IsFinite()) + { + if (pos.Z > 9999999f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (pos.Z < -90000f) + { + pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + _position = pos; + } + else + { + _position = new Vector3(((float)_parent_scene.WorldExtents.X * 0.5f), ((float)_parent_scene.WorldExtents.Y * 0.5f), parent_scene.GetTerrainHeightAtXY(128f, 128f) + 10f); + m_log.Warn("[PHYSICS]: Got NaN Position on Character Create"); + } + + _parent_scene = parent_scene; + + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_density = density; + m_mass = 80f; // sure we have a default + + AvatarContactData.mu = parent_scene.AvatarFriction; + AvatarContactData.bounce = parent_scene.AvatarBounce; + + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + CAPSULE_LENGTH = size.Z * 1.15f - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; + + m_isPhysical = false; // current status: no ODE information exists + m_tainted_isPhysical = true; // new tainted status: need to create ODE information + + m_hasTaintForce = false; + _parent_scene.AddPhysicsActorTaint(this); + + m_name = avName; + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override ContactData ContactData + { + get { return AvatarContactData; } + } + + public override bool Building { get; set; } + + /// + /// If this is set, the avatar will move faster + /// + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override bool FloatOnWater + { + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return flying; } + set + { + flying = value; + // m_log.DebugFormat("[PHYSICS]: Set OdeCharacter Flying to {0}", flying); + } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return (m_iscolliding || m_iscollidingGround); } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + { +// SetPidStatus(false); + m_pidControllerActive = true; + m_iscolliding = true; + } + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + /* we now control this + if (value) + { + m_colliderGroundfilter += 2; + if (m_colliderGroundfilter > 2) + m_colliderGroundfilter = 2; + } + else + { + m_colliderGroundfilter--; + if (m_colliderGroundfilter < 0) + m_colliderGroundfilter = 0; + } + + if (m_colliderGroundfilter == 0) + m_iscollidingGround = false; + else + m_iscollidingGround = true; + */ + } + + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + // Ubit filter this also + if (value) + { + m_colliderObjectfilter += 2; + if (m_colliderObjectfilter > 2) + m_colliderObjectfilter = 2; + } + else + { + m_colliderObjectfilter--; + if (m_colliderObjectfilter < 0) + m_colliderObjectfilter = 0; + } + + if (m_colliderObjectfilter == 0) + m_iscollidingObj = false; + else + m_iscollidingObj = true; + + // m_iscollidingObj = value; +/* + if (m_iscollidingObj) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + */ + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + /// + /// This 'puts' an avatar somewhere in the physics space. + /// Not really a good choice unless you 'know' it's a good + /// spot otherwise you're likely to orbit the avatar. + /// + public override Vector3 Position + { + get { return _position; } + set + { + if (Body == IntPtr.Zero || Shell == IntPtr.Zero) + { + if (value.IsFinite()) + { + if (value.Z > 9999999f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + if (value.Z < -90000f) + { + value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; + } + + m_taintPosition.X = value.X; + m_taintPosition.Y = value.Y; + m_taintPosition.Z = value.Z; + m_hasTaintPosition = true; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Position from Scene on a Character"); + } + } + } + } + + public override Vector3 RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + /// + /// This property sets the height of the avatar only. We use the height to make sure the avatar stands up straight + /// and use it to offset landings properly + /// + public override Vector3 Size + { + get { + float d = CAPSULE_RADIUS * 2; + return new Vector3(d, d, (CAPSULE_LENGTH +d)/1.15f); } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + + Vector3 SetSize = value; + m_tainted_CAPSULE_LENGTH = SetSize.Z *1.15f - CAPSULE_RADIUS * 2.0f; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); + } + } + } + + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + + m_mass = ShellMass.mass; // update mass + + // rescale PID parameters + PID_D = _parent_scene.avPIDD; + PID_P = _parent_scene.avPIDP; + + // rescale PID parameters so that this aren't affected by mass + // and so don't get unstable for some masses + // also scale by ode time step so you don't need to refix them + + PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps + PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_P /= 50 * 80; + PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + m_hasTaintPosition = false; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(Shell, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0 , 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geometry + if (Shell != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + // + /// + /// Uses the capped cyllinder volume formula to calculate the avatar's mass. + /// This may be used in calculations in the scene/scenepresence + /// + public override float Mass + { + get + { + float AVvolume = (float)(Math.PI * CAPSULE_RADIUS * CAPSULE_RADIUS * (1.3333333333f * CAPSULE_RADIUS + CAPSULE_LENGTH)); + return m_density * AVvolume; + } + } + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(Vector3 axis) + { + + } + + + public override Vector3 Force + { + get { return _target_velocity; } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void VehicleFlags(int param, bool remove) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override Vector3 CenterOfMass + { + get + { + Vector3 pos = _position; + return pos; + } + } + + public override Vector3 GeometricCenter + { + get + { + Vector3 pos = _position; + return pos; + } + } + + //UBit mess + /* for later use + public override Vector3 PrimOOBsize + { + get + { + Vector3 s=Size; + s.X *=0.5f; + s.Y *=0.5f; + s.Z *=0.5f; + return s; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return Vector3.Zero; + } + } + */ + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override Vector3 Velocity + { + get + { + return _velocity; + } + set + { + if (value.IsFinite()) + { + m_pidControllerActive = true; + _target_velocity = value; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN velocity from Scene in a Character"); + } + } + } + + public override Vector3 Torque + { + get { return Vector3.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set + { + //Matrix3 or = Orientation.ToRotationMatrix(); + //d.Matrix3 ord = new d.Matrix3(or.m00, or.m10, or.m20, or.m01, or.m11, or.m21, or.m02, or.m12, or.m22); + //d.BodySetRotation(Body, ref ord); + } + } + + 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) + { + m_pidControllerActive = false; + m_taintForce = force / _parent_scene.ODE_STEPSIZE; + m_hasTaintForce = true; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_pidControllerActive = true; + _target_velocity.X += force.X; + _target_velocity.Y += force.Y; + _target_velocity.Z += force.Z; + } + } + 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) + { + } + + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep, List defects) + { + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if (Body == IntPtr.Zero) + return; + + d.Vector3 dtmp; + d.BodyCopyPosition(Body, out dtmp); + Vector3 localpos = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + // the Amotor still lets avatar rotation to drift during colisions + // so force it back to identity + + d.Quaternion qtmp; + qtmp.W = 1; + qtmp.X = 0; + qtmp.Y = 0; + qtmp.Z = 0; + d.BodySetQuaternion(Body, ref qtmp); + + if (m_pidControllerActive == false) + { + _zeroPosition = localpos; + } + //PidStatus = true; + + + if (!localpos.IsFinite()) + { + + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + + return; + } + + Vector3 vec = Vector3.Zero; + dtmp = d.BodyGetLinearVel(Body); + Vector3 vel = new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + + float movementdivisor = 1f; + //Ubit change divisions into multiplications below + if (!m_alwaysRun) + { + movementdivisor = 1 / walkDivisor; + } + else + { + movementdivisor = 1 / runDivisor; + } + + // colide with land + + d.AABB aabb; + d.GeomGetAABB(Shell, out aabb); + float chrminZ = aabb.MinZ; + + Vector3 posch = localpos; + + float ftmp; + + if (flying) + { + ftmp = timeStep; + posch.X += vel.X * ftmp; + posch.Y += vel.Y * ftmp; + } + + float terrainheight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + if (chrminZ < terrainheight) + { + float depth = terrainheight - chrminZ; + if (!flying) + { + vec.Z = -vel.Z * PID_D * 1.5f + depth * PID_P * 50; + } + else + vec.Z = depth * PID_P * 50; + + /* + Vector3 vtmp; + vtmp.X = _target_velocity.X * timeStep; + vtmp.Y = _target_velocity.Y * timeStep; + // fake and avoid squares + float k = (Math.Abs(vtmp.X) + Math.Abs(vtmp.Y)); + if (k > 0) + { + posch.X += vtmp.X; + posch.Y += vtmp.Y; + terrainheight -= _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y); + k = 1 + Math.Abs(terrainheight) / k; + movementdivisor /= k; + + if (k < 1) + k = 1; + } + */ + + + if (depth < 0.1f) + { + m_iscolliding = true; + m_colliderfilter = 2; + m_iscollidingGround = true; + + ContactPoint contact = new ContactPoint(); + contact.PenetrationDepth = depth; + contact.Position.X = localpos.X; + contact.Position.Y = localpos.Y; + contact.Position.Z = chrminZ; + contact.SurfaceNormal.X = 0f; + contact.SurfaceNormal.Y = 0f; + contact.SurfaceNormal.Z = -1f; + AddCollisionEvent(0, contact); + + vec.Z *= 0.5f; + } + + else + m_iscollidingGround = false; + } + else + m_iscollidingGround = false; + + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f + && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = localpos; + } + if (m_pidControllerActive) + { + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + vec.X = -vel.X * PID_D + (_zeroPosition.X - localpos.X) * (PID_P * 2); + vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - localpos.Y) * (PID_P * 2); + if (flying) + { + vec.Z += -vel.Z * PID_D + (_zeroPosition.Z - localpos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + _zeroFlag = false; + + if (m_iscolliding) + { + if (!flying) + { + if (_target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. JUMPING + vec.Z += (_target_velocity.Z - vel.Z) * PID_D * 1.2f;// +(_zeroPosition.Z - localpos.Z) * PID_P; + } + // We're standing on something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D); + } + else + { + // We're flying and colliding with something + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 0.0625f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 0.0625f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + } + else // ie not colliding + { + if (flying) //(!m_iscolliding && flying) + { + // we're in mid air suspended + vec.X = ((_target_velocity.X * movementdivisor) - vel.X) * (PID_D * 1.667f); + vec.Y = ((_target_velocity.Y * movementdivisor) - vel.Y) * (PID_D * 1.667f); + vec.Z += (_target_velocity.Z - vel.Z) * (PID_D); + } + + else + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D * 0.833f; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.833f; + } + } + } + + if (flying) + { + vec.Z -= _parent_scene.gravityz * m_mass; + + //Added for auto fly height. Kitto Flora + float target_altitude = _parent_scene.GetTerrainHeightAtXY(localpos.X, localpos.Y) + MinimumGroundFlightOffset; + + if (localpos.Z < target_altitude) + { + vec.Z += (target_altitude - localpos.Z) * PID_P * 5.0f; + } + // end add Kitto Flora + } + + if (vec.IsFinite()) + { + if (vec.X != 0 || vec.Y !=0 || vec.Z !=0) + d.BodyAddForce(Body, vec.X, vec.Y, vec.Z); + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()"); + m_log.Warn("[PHYSICS]: Avatar Position is non-finite!"); + defects.Add(this); + // _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (Body == IntPtr.Zero) + return; + + d.Vector3 vec; + try + { + d.BodyCopyPosition(Body, out vec); + } + catch (NullReferenceException) + { + bad = true; + _parent_scene.BadCharacter(this); + vec = new d.Vector3(_position.X, _position.Y, _position.Z); + base.RaiseOutOfBounds(_position); // Tells ScenePresence that there's a problem! + m_log.WarnFormat("[ODEPLUGIN]: Avatar Null reference for Avatar {0}, physical actor {1}", m_name, m_uuid); + } + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + bool fixbody = false; + + if (_position.X < 0.0f) + { + fixbody = true; + _position.X = 0.1f; + } + else if (_position.X > (int)_parent_scene.WorldExtents.X - 0.1f) + { + fixbody = true; + _position.X = (int)_parent_scene.WorldExtents.X - 0.1f; + } + + if (_position.Y < 0.0f) + { + fixbody = true; + _position.Y = 0.1f; + } + else if (_position.Y > (int)_parent_scene.WorldExtents.Y - 0.1) + { + fixbody = true; + _position.Y = (int)_parent_scene.WorldExtents.Y - 0.1f; + } + + if (fixbody) + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + // Did we move last? = zeroflag + // This helps keep us from sliding all over +/* + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); + } + } + else + { + m_lastUpdateSent = false; + */ + try + { + vec = d.BodyGetLinearVel(Body); + } + catch (NullReferenceException) + { + vec.X = _velocity.X; + vec.Y = _velocity.Y; + vec.Z = _velocity.Z; + } + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + _velocity.Z = (vec.Z); + // } + } + + /// + /// Cleanup the things we use in the scene. + /// + public void Destroy() + { + m_tainted_isPhysical = false; + _parent_scene.AddPhysicsActorTaint(this); + } + + public override void CrossingFailure() + { + } + + public override Vector3 PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override float PIDHoverHeight { set { return; } } + public override bool PIDHoverActive { set { return; } } + public override PIDHoverType PIDHoverType { set { return; } } + public override float PIDHoverTau { set { return; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + + public override void SubscribeEvents(int ms) + { + m_requestedUpdateFrequency = ms; + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + m_haseventsubscription = true; + } + + public override void UnSubscribeEvents() + { + m_haseventsubscription = false; + _parent_scene.RemoveCollisionEventReporting(this); + m_requestedUpdateFrequency = 0; + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (m_haseventsubscription) + { + // m_log.DebugFormat( + // "[PHYSICS]: Adding collision event for {0}, collidedWith {1}, contact {2}", "", CollidedWith, contact); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + } + + public void SendCollisions() + { + if (m_haseventsubscription && m_eventsubscription > m_requestedUpdateFrequency) + { + if (CollisionEventsThisFrame != null) + { + base.SendCollisionUpdate(CollisionEventsThisFrame); + } + CollisionEventsThisFrame = new CollisionEventUpdate(); + m_eventsubscription = 0; + } + } + + public override bool SubscribedEvents() + { + return m_haseventsubscription; + } + + public void ProcessTaints(float timestep) + { + + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) + { + // Create avatar capsule and related ODE data + if ((Shell != IntPtr.Zero)) + { + // a lost shell ? + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell != IntPtr.Zero ? "Shell " : "") + + (Body != IntPtr.Zero ? "Body " : "") + + (Amotor != IntPtr.Zero ? "Amotor " : "")); + AvatarGeomAndBodyDestroy(); + } + + AvatarGeomAndBodyCreation(_position.X, _position.Y, _position.Z); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + _parent_scene.AddCharacter(this); + } + else + { + _parent_scene.RemoveCharacter(this); + // destroy avatar capsule and related ODE data + AvatarGeomAndBodyDestroy(); + } + + m_isPhysical = m_tainted_isPhysical; + } + + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + { + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + AvatarGeomAndBodyDestroy(); + + m_pidControllerActive = true; + + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + + Velocity = Vector3.Zero; + + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == IntPtr.Zero ? "Shell " : "") + + (Body == IntPtr.Zero ? "Body " : "") + + (Amotor == IntPtr.Zero ? "Amotor " : "")); + } + } + + if (m_hasTaintPosition) + { + if (Body != IntPtr.Zero) + d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + + _position.X = m_taintPosition.X; + _position.Y = m_taintPosition.Y; + _position.Z = m_taintPosition.Z; + m_hasTaintPosition = false; + } + + if (m_hasTaintForce) + { + if (Body != IntPtr.Zero) + { + if(m_taintForce.X !=0f || m_taintForce.Y !=0f || m_taintForce.Z !=0) + d.BodyAddForce(Body, m_taintForce.X, m_taintForce.Y, m_taintForce.Z); + m_hasTaintForce = false; + } + } + + } + + internal void AddCollisionFrameTime(int p) + { + // protect it from overflow crashing + if (m_eventsubscription + p >= int.MaxValue) + m_eventsubscription = 0; + m_eventsubscription += p; + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments new file mode 100644 index 0000000..1060aa6 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments @@ -0,0 +1,630 @@ +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + * + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + + public IntPtr Body + { + get { return m_body; } + } + + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private OdeScene m_parentScene = null; + private IntPtr m_body = IntPtr.Zero; + private IntPtr m_jointGroup = IntPtr.Zero; + private IntPtr m_aMotor = IntPtr.Zero; + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; + private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; + private Vector3 m_angularFrictionTimescale = Vector3.Zero; + private float m_angularMotorDecayTimescale = 0; + private float m_angularMotorTimescale = 0; + private Vector3 m_lastAngularVelocityVector = Vector3.Zero; + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 0; + private float m_verticalAttractionTimescale = 0; + + + + + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.0f) pValue = 0.0f; + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + } + + }//end ProcessRotationVehicleParam + + internal void ProcessTypeChange(Vehicle pType) + { +Console.WriteLine("ProcessTypeChange to " + pType); + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 10; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1; + m_verticalAttractionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Enable(IntPtr pBody, OdeScene pParentScene) + { +//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy); + if (m_type == Vehicle.TYPE_NONE) + return; + + m_body = pBody; + //KF: This used to set up the linear and angular joints + } + + internal void Step(float pTimestep, OdeScene pParentScene) + { + if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE) + return; + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, OdeScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + if(m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + d.Vector3 vel_now = d.BodyGetLinearVel(Body); + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + } // else its 1.0, no gravity. + + // Check if hovering + if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; +//if(frcount == 0) Console.WriteLine("herr0=" + herr0); + // Replace Vertical speed with correction figure if significant + if(Math.Abs(herr0) > 0.01f ) + { + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + // m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + + // Apply velocity + d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z); +//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); +//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z); + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + + // m_angularMotorDirection is the latest value from the script, and is decayed here + // m_angularMotorDirectionLASTSET is the latest value from the script + // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here + + if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) + { + if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + // ramp up to new value + Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); + m_lastAngularVelocityVector += (addAmount * 10f); +//if(frcount == 0) Console.WriteLine("add: " + addAmount); + + // limit applied value to what was set by script + // This will work temporarily, but we really need to compare speed on an axis + if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) + m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; + if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) + m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) + m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; + + // decay the requested value + Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_angularMotorDirection -= m_angularMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ? + + // Vertical attractor section + +// d.Mass objMass; +// d.BodyGetMass(Body, out objMass); +// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by servo + verterr = verterr * servo; + + // rotate to object frame + // verterr = verterr * rotq; + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + m_lastAngularVelocityVector.X += verterr.Y; + m_lastAngularVelocityVector.Y -= verterr.X; +/* +if(frcount == 0) + { +// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector); + Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}", + Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency)); + } + */ + d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z); + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; + + } //end MoveAngular + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs new file mode 100644 index 0000000..363cbef --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -0,0 +1,1035 @@ +/* + * 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. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class ODEDynamics + { + public Vehicle Type + { + get { return m_type; } + } + +// private OdeScene m_parentScene = null; +// private IntPtr m_aMotor = IntPtr.Zero; + + + private OdePrim rootPrim; + private OdeScene _pParentScene; + + + + + private Vector3 refUpAxis = new Vector3(0, 0, 1); + private Vector3 refAtAxis = new Vector3(1, 0, 0); + + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl + private Quaternion m_RollreferenceFrame = Quaternion.Identity; + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + private float m_linearMotorDecayTimescale = 120; + private float m_linearMotorTimescale = 1000; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + + //Deflection properties + private float m_angularDeflectionEfficiency = 0; + private float m_angularDeflectionTimescale = 1000; + private float m_linearDeflectionEfficiency = 0; + private float m_linearDeflectionTimescale = 1000; + + //Banking properties + private float m_bankingEfficiency = 0; + private float m_bankingMix = 0; + private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 1000f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. + + // special contact data for vehicles + public ContactData VehiculeContactData = new ContactData(0f, 0.1f); + + // auxiliar + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + + private float m_lmEfect = 0; // current linear motor eficiency + private float m_amEfect = 0; // current angular motor eficiency + + + public ODEDynamics(OdePrim rootp) + { + rootPrim = rootp; + _pParentScene = rootPrim._parent_scene; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + m_angularMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = pValue * invtimestep; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float invtimestep = 1.0f / _pParentScene.ODE_STEPSIZE; + float timestep = _pParentScene.ODE_STEPSIZE; + switch (pParam) + { + + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + len = m_angularMotorDirection.Length(); + if (len > 12.566f) + m_angularMotorDirection *= (12.566f / len); + m_amEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorDirection.Length(); + if (len > 30.0f) + m_linearMotorDirection *= (30.0f / len); + m_lmEfect = 1.0f; // turn it on + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = m_linearMotorOffset.Length(); + if (len > 100.0f) + m_linearMotorOffset *= (100.0f / len); + break; + case Vehicle.BLOCK_EXIT: + m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + m_referenceFrame = Quaternion.Inverse(pValue); + break; + case Vehicle.ROLL_FRAME: + m_RollreferenceFrame = pValue; + break; + } + }//end ProcessRotationVehicleParam + + internal void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + internal void ProcessTypeChange(Vehicle pType) + { + float invtimestep = _pParentScene.ODE_STEPSIZE; + m_lmEfect = 0; + m_amEfect = 0; + + m_linearMotorDirection = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + + m_BlockingEndPoint = Vector3.Zero; + m_RollreferenceFrame = Quaternion.Identity; + m_linearMotorOffset = Vector3.Zero; + + m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000 * invtimestep; + m_VhoverHeight = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120 * invtimestep; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 1; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 1000; + m_bankingEfficiency = 0; + m_bankingMix = 1; + m_bankingTimescale = 10; + m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 2; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + m_bankingEfficiency = -0.2f; + m_bankingMix = 1; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 3; + m_angularDeflectionEfficiency = 0.5f; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + m_bankingEfficiency = -0.3f; + m_bankingMix = 0.8f; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_ROLL_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8 * invtimestep; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 0.5f; + m_angularDeflectionEfficiency = 1; + m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + m_bankingEfficiency = 1; + m_bankingMix = 0.7f; + m_bankingTimescale = 2; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60 * invtimestep; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10 * invtimestep; + m_VhoverHeight = 5; + m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0; + m_linearDeflectionTimescale = 5 * invtimestep; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0f; + m_verticalAttractionTimescale = 1000f; + m_bankingEfficiency = 0; + m_bankingMix = 0.7f; + m_bankingTimescale = 5; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + } + + }//end SetDefaultsForType + + internal void Stop() + { + m_lmEfect = 0; + m_amEfect = 0; + } + + public static Vector3 Xrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.X + rot.W * rot.W) - 1; + vec.Y = 2 * (rot.X * rot.Y + rot.Z * rot.W); + vec.Z = 2 * (rot.X * rot.Z - rot.Y * rot.W); + return vec; + } + + public static Vector3 Zrot(Quaternion rot) + { + Vector3 vec; + rot.Normalize(); // just in case + vec.X = 2 * (rot.X * rot.Z + rot.Y * rot.W); + vec.Y = 2 * (rot.Y * rot.Z - rot.X * rot.W); + vec.Z = 2 * (rot.Z * rot.Z + rot.W * rot.W) - 1; + + return vec; + } + + private const float halfpi = 0.5f * (float)Math.PI; + + public static Vector3 ubitRot2Euler(Quaternion rot) + { + // returns roll in X + // pitch in Y + // yaw in Z + Vector3 vec; + + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + vec.X = 0; + vec.Y = -halfpi; + vec.Z = (float)(-2d * Math.Atan(rot.X / rot.W)); + } + else if (zX > 0.49999f) + { + vec.X = 0; + vec.Y = halfpi; + vec.Z = (float)(2d * Math.Atan(rot.X / rot.W)); + } + else + { + vec.Y = (float)Math.Asin(2 * zX); + + float sqw = rot.W * rot.W; + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + sqw - 0.5f; + + vec.X = (float)Math.Atan2(minuszY, zZ); + + float yX = rot.Z * rot.W - rot.X * rot.Y; //( have negative ?) + float yY = rot.X * rot.X + sqw - 0.5f; + vec.Z = (float)Math.Atan2(yX, yY); + } + return vec; + } + + + public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) + { + // assuming rot is normalised + // rot.Normalize(); + + float zX = rot.X * rot.Z + rot.Y * rot.W; + + if (zX < -0.49999f) + { + roll = 0; + pitch = -halfpi; + } + else if (zX > 0.49999f) + { + roll = 0; + pitch = halfpi; + } + else + { + pitch = (float)Math.Asin(2 * zX); + + float minuszY = rot.X * rot.W - rot.Y * rot.Z; + float zZ = rot.Z * rot.Z + rot.W * rot.W - 0.5f; + + roll = (float)Math.Atan2(minuszY, zZ); + } + return ; + } + + internal void Step()//float pTimestep) + { + IntPtr Body = rootPrim.Body; + + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion objrotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion rotq = objrotq; // rotq = rotation of object + rotq *= m_referenceFrame; // rotq is now rotation in vehicle reference frame + Quaternion irotq = Quaternion.Inverse(rotq); + + d.Vector3 dvtmp; + Vector3 tmpV; + dvtmp = d.BodyGetLinearVel(Body); + Vector3 curVel; + curVel.X = dvtmp.X; + curVel.Y = dvtmp.Y; + curVel.Z = dvtmp.Z; + Vector3 curLocalVel = curVel * irotq; // current velocity in local + + dvtmp = d.BodyGetAngularVel(Body); + Vector3 curAngVel; + curAngVel.X = dvtmp.X; + curAngVel.Y = dvtmp.Y; + curAngVel.Z = dvtmp.Z; + Vector3 curLocalAngVel = curAngVel * irotq; // current velocity in local + + 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 object frame + d.Vector3 dtorque = new d.Vector3();// actually angular aceleration until mult by Inertia in object frame + + bool doathing = false; + + // linear motor + if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) + { + tmpV = m_linearMotorDirection - curLocalVel; // velocity error + if (tmpV.LengthSquared() > 1e-6f) + { + tmpV = tmpV * (m_lmEfect / m_linearMotorTimescale); // error to correct in this timestep + tmpV *= rotq; // to world + + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0) + tmpV.Z = 0; + + if (m_linearMotorOffset.X != 0 && m_linearMotorOffset.Y != 0 && m_linearMotorOffset.Z != 0) + { + // have offset, do it now + tmpV *= rootPrim.Mass; + d.BodyAddForceAtRelPos(Body, tmpV.X, tmpV.Y, tmpV.Z, m_linearMotorOffset.X, m_linearMotorOffset.Y, m_linearMotorOffset.Z); + } + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + } + m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale); + } + else + m_lmEfect = 0; + + // friction + if (curLocalVel.X != 0 || curLocalVel.Y != 0 || curLocalVel.Z != 0) + { + tmpV.X = -curLocalVel.X / m_linearFrictionTimescale.X; + tmpV.Y = -curLocalVel.Y / m_linearFrictionTimescale.Y; + tmpV.Z = -curLocalVel.Z / m_linearFrictionTimescale.Z; + tmpV *= rotq; // to world + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + + // hover + if (m_VhoverTimescale < 300) + { + d.Vector3 pos = d.BodyGetPosition(Body); + + // default to global + float perr = m_VhoverHeight - pos.Z;; + + if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + perr += _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + } + else if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + perr += _pParentScene.GetWaterLevel(); + } + else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == 0) + { + float t = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float w = _pParentScene.GetWaterLevel(); + if (t > w) + perr += t; + else + perr += w; + } + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == 0 || perr > 0) + { + force.Z += (perr / m_VhoverTimescale / m_VhoverTimescale - curVel.Z * m_VhoverEfficiency) / _pParentScene.ODE_STEPSIZE; + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + else // no buoyancy + force.Z += _pParentScene.gravityz; + } + else + { + // default gravity and buoancy + force.Z += _pParentScene.gravityz * (1f - m_VehicleBuoyancy); + } + + // linear deflection + if (m_linearDeflectionEfficiency > 0) + { + float len = curVel.Length(); + Vector3 atAxis = refAtAxis; + atAxis *= rotq; // at axis rotated to world + atAxis = Xrot(rotq); + tmpV = atAxis * len; + tmpV -= curVel; // velocity error + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if((m_flags & VehicleFlag.NO_DEFLECTION_UP) ==0) + force.Z += tmpV.Z; + } + + // angular motor + if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) + { + tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error + if (tmpV.LengthSquared() > 1e-6f) + { + tmpV = tmpV * (m_amEfect / m_angularMotorTimescale); // error to correct in this timestep + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); + } + else + m_amEfect = 0; + + // angular friction + if (curLocalAngVel.X != 0 || curLocalAngVel.Y != 0 || curLocalAngVel.Z != 0) + { + tmpV.X = -curLocalAngVel.X / m_angularFrictionTimescale.X; + tmpV.Y = -curLocalAngVel.Y / m_angularFrictionTimescale.Y; + tmpV.Z = -curLocalAngVel.Z / m_angularFrictionTimescale.Z; + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + + // angular deflection + if (m_angularDeflectionEfficiency > 0) + { + doathing = false; + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE; + tmpV.X = 0; + if (Math.Abs(curLocalVel.Z) > 0.01) + { + tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp; + doathing = true; + } + else + tmpV.Y = 0; + if (Math.Abs(curLocalVel.Y) > 0.01) + { + tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp; + doathing = true; + } + else + tmpV.Z = 0; + + if (doathing) + { + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + } + + // vertical atractor + if (m_verticalAttractionTimescale < 300) + { + doathing = false; + float roll; + float pitch; + + GetRollPitch(rotq, out roll, out pitch); + + + float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; + float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; + + if (Math.Abs(roll) > 0.01) // roll + { + tmpV.X = -roll * ftmp; + tmpV.X -= curLocalAngVel.X * ftmp2; + doathing = true; + } + else + { + tmpV.X = 0; + } + + if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch + { + tmpV.Y = -pitch * ftmp; + tmpV.Y -= curLocalAngVel.Y * ftmp2; + doathing = true; + } + else + { + tmpV.Y = 0; + } + + tmpV.Z = 0; + + if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01) + tmpV.Z = 0; + else + { + float broll = -roll * m_bankingEfficiency; ; + if (m_bankingMix != 0) + { + float vfact = m_bankingMix * Math.Abs(curLocalVel.X) / 10.0f; + if (vfact < m_bankingMix) + broll *= ((1 - m_bankingMix) + vfact); + } + + tmpV.Z = (broll - curLocalAngVel.Z) / m_bankingTimescale; + doathing = true; + } + + if (doathing) + { + + tmpV *= m_referenceFrame; // to object + dtorque.X += tmpV.X; + dtorque.Y += tmpV.Y; + dtorque.Z += tmpV.Z; + } + } + /* + d.Vector3 pos = d.BodyGetPosition(Body); + // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); + Vector3 posChange = new Vector3(); + posChange.X = pos.X - m_lastPositionVector.X; + posChange.Y = pos.Y - m_lastPositionVector.Y; + posChange.Z = pos.Z - m_lastPositionVector.Z; + double Zchange = Math.Abs(posChange.Z); + if (m_BlockingEndPoint != Vector3.Zero) + { + if (pos.X >= (m_BlockingEndPoint.X - (float)1)) + { + pos.X -= posChange.X + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Y >= (m_BlockingEndPoint.Y - (float)1)) + { + pos.Y -= posChange.Y + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Z >= (m_BlockingEndPoint.Z - (float)1)) + { + pos.Z -= posChange.Z + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.X <= 0) + { + pos.X += posChange.X + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + if (pos.Y <= 0) + { + pos.Y += posChange.Y + 1; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + } + if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y)) + { + pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2; + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + } + + } + if ((m_flags & (VehicleFlag.NO_X)) != 0) + { + m_dir.X = 0; + } + if ((m_flags & (VehicleFlag.NO_Y)) != 0) + { + m_dir.Y = 0; + } + if ((m_flags & (VehicleFlag.NO_Z)) != 0) + { + m_dir.Z = 0; + } + + + */ + // angular part + /* + + // Get what the body is doing, this includes 'external' influences +/* + Vector3 angularVelocity = Vector3.Zero; + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 0.2f / m_verticalAttractionTimescale; + // get present body rotation + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by VAservo + verterr = verterr * VAservo; +//if (frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; + + // scaling appears better usingsquare-law + float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); + vertattr.X += bounce * angularVelocity.X; + vertattr.Y += bounce * angularVelocity.Y; + + } // else vertical attractor is off + + // m_lastVertAttractor = vertattr; + + // Bank section tba + // Deflection section tba + + // Sum velocities + m_lastAngularVelocity = angularVelocity + vertattr; // + bank + deflection + + if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) + { + m_lastAngularVelocity.X = 0; + m_lastAngularVelocity.Y = 0; + } + + if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + { + if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); + } + else + { + m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. + } +*/ + + d.Mass dmass; + d.BodyGetMass(Body,out dmass); + + if (force.X != 0 || force.Y != 0 || force.Z != 0) + { + force *= dmass.mass; + d.BodySetForce(Body, force.X, force.Y, force.Z); + } + + if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0) + { + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); + d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame + } + + //end MoveAngular + + // limit rotations +/* + bool changed = false; + + if (m_RollreferenceFrame != Quaternion.Identity) + { + if (rotq.X >= m_RollreferenceFrame.X) + { + rot.X = rotq.X - (m_RollreferenceFrame.X / 2); + } + if (rotq.Y >= m_RollreferenceFrame.Y) + { + rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2); + } + if (rotq.X <= -m_RollreferenceFrame.X) + { + rot.X = rotq.X + (m_RollreferenceFrame.X / 2); + } + if (rotq.Y <= -m_RollreferenceFrame.Y) + { + rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2); + } + changed = true; + } + + if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) + { + rot.X = 0; + rot.Y = 0; + changed = true; + } + if (changed) + d.BodySetQuaternion(Body, ref rot); +*/ + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs new file mode 100644 index 0000000..1a53c99 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -0,0 +1,3260 @@ +/* + * 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 by Ubit Umarov + * + * + */ + +/* + * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using log4net; +using OpenMetaverse; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdePrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private bool m_isphysical; + private bool m_fakeisphysical; + + protected bool m_building; + private Quaternion m_lastorientation = new Quaternion(); + private Quaternion _orientation; + + private Vector3 _position; + private Vector3 _velocity; + private Vector3 _torque; + private Vector3 m_lastVelocity; + private Vector3 m_lastposition; + private Vector3 m_rotationalVelocity; + private Vector3 _size; + private Vector3 _acceleration; + private Vector3 m_angularlock = Vector3.One; + private IntPtr Amotor = IntPtr.Zero; + + private Vector3 m_force; + private Vector3 m_forceacc; + private Vector3 m_angularForceacc; + + private Vector3 m_PIDTarget; + private float m_PIDTau; + private float PID_D = 35f; + private float PID_G = 25f; + private bool m_usePID; + + // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), + // and are for non-VEHICLES only. + + private float m_PIDHoverHeight; + private float m_PIDHoverTau; + private bool m_useHoverPID; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight; + private float m_groundHeight; + private float m_waterHeight; + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + + private int body_autodisable_frames = 20; + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + private bool m_collidesLand = true; + private bool m_collidesWater; + public bool m_returnCollisions; + + // Default we're a Geometry + private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); + + // Default, Collide with Other Geometries, spaces and Bodies + private CollisionCategories m_collisionFlags = m_default_collisionFlags; + + public bool m_disabled; + public bool m_taintselected; + + public uint m_localID; + + private PrimitiveBaseShape _pbs; + public OdeScene _parent_scene; + + /// + /// The physics space which contains prim geometry + /// + public IntPtr m_targetSpace = IntPtr.Zero; + + public IntPtr prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent; + + private List childrenPrim = new List(); + + private bool m_iscolliding; + private bool m_wascolliding; + private bool m_isSelected; + + internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates; + private int throttleCounter; + public int m_interpenetrationcount; + public float m_collisionscore; + int m_colliderfilter = 0; + public int m_roundsUnderMotionThreshold; + private int m_crossingfailures; + + public bool outofBounds; + private float m_density = 10.000006836f; // Aluminum g/cm3; + + public bool _zeroFlag; + private bool m_lastUpdateSent; + + public IntPtr Body = IntPtr.Zero; + public String Name { get; private set; } + private Vector3 _target_velocity; + + public Vector3 primOOBsize; // prim real dimensions from mesh + public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb + public float primOOBradiusSQ; + public d.Mass primdMass; // prim inertia information on it's own referencial + float primMass; // prim own mass + float _mass; // object mass acording to case + public d.Mass objectpMass; // object last computed inertia + private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb + + public int givefakepos = 0; + private Vector3 fakepos; + public int givefakeori = 0; + private Quaternion fakeori; + + public int m_eventsubscription; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + private IntPtr m_linkJoint = IntPtr.Zero; + private IntPtr _linkJointGroup = IntPtr.Zero; + + public volatile bool childPrim; + + public ODEDynamics m_vehicle; + + internal int m_material = (int)Material.Wood; + protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f}; + + /// + /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. + /// + public override bool IsPhysical // this is not reliable for internal use + { + get { return m_fakeisphysical; } + set + { + m_fakeisphysical = value; // we show imediatly to outside that we changed physical + // and also to stop imediatly some updates + // but real change will only happen in taintprocessing + + if (!value) // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + AddChange(changes.Physical, value); + } + } + + public override bool Building // this is not reliable for internal use + { + get { return m_building; } + set + { + if (value) + m_building = true; + AddChange(changes.building, value); + } + } + + public override ContactData ContactData + { + get + { +/* + ODEDynamics v; + if(childPrim && _parent !=null) + { + v =((OdePrim)_parent).m_vehicle; + if(v != null && v.Type != Vehicle.TYPE_NONE) + return v.VehiculeContactData; + return primContactData; + } + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + return m_vehicle.VehiculeContactData; +*/ + return primContactData; + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override uint LocalID + { + get + { + return m_localID; + } + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + if(value) + m_isSelected = value; + AddChange(changes.Selected, value); + } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + if (value) + { + m_colliderfilter += 2; + if (m_colliderfilter > 2) + m_colliderfilter = 2; + } + else + { + m_colliderfilter--; + if (m_colliderfilter < 0) + m_colliderfilter = 0; + } + + if (m_colliderfilter == 0) + m_iscolliding = false; + else + m_iscolliding = true; + + if (m_wascolliding != m_iscolliding) + { + if (m_wascolliding && !m_isSelected && Body != IntPtr.Zero) + d.BodyEnable(Body); + m_wascolliding = m_iscolliding; + } + } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override Vector3 Position + { + get + { + if (givefakepos > 0) + return fakepos; + else + return _position; + } + + set + { + fakepos = value; + givefakepos++; + AddChange(changes.Position, value); + } + } + + public override Vector3 Size + { + get { return _size; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Size, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); + } + } + } + + public override float Mass + { + get { return _mass; } + } + + public override Vector3 Force + { + //get { return Vector3.Zero; } + get { return m_force; } + set + { + if (value.IsFinite()) + { + AddChange(changes.Force, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); + } + } + } + + + public override void SetVolumeDetect(int param) + { + AddChange(changes.VolumeDtc,(param != 0)); + } + + public override Vector3 GeometricCenter + { + get + { + return Vector3.Zero; + } + } + + public override Vector3 CenterOfMass + { + get + { + d.Vector3 dtmp; + if (IsPhysical && !childPrim && Body != IntPtr.Zero) + { + dtmp = d.BodyGetPosition(Body); + return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); + } + else if (prim_geom != IntPtr.Zero) + { + d.Quaternion dq; + d.GeomCopyQuaternion(prim_geom, out dq); + Quaternion q; + q.X = dq.X; + q.Y = dq.Y; + q.Z = dq.Z; + q.W = dq.W; + + Vector3 vtmp = primOOBoffset * q; + dtmp = d.GeomGetPosition(prim_geom); + return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); + } + else + return Vector3.Zero; + } + } + /* + public override Vector3 PrimOOBsize + { + get + { + return primOOBsize; + } + } + + public override Vector3 PrimOOBoffset + { + get + { + return primOOBoffset; + } + } + + public override float PrimOOBRadiusSQ + { + get + { + return primOOBradiusSQ; + } + } + */ + public override PrimitiveBaseShape Shape + { + set + { + AddChange(changes.Shape, value); + } + } + + public override Vector3 Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + if (_zeroFlag) + return Vector3.Zero; +/* + Vector3 returnVelocity = Vector3.Zero; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; + + return returnVelocity; + */ + return _velocity; + } + set + { + if (value.IsFinite()) + { + AddChange(changes.Velocity, value); +// _velocity = value; + + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); + } + + } + } + + public override Vector3 Torque + { + get + { + if (!IsPhysical || Body == IntPtr.Zero) + return Vector3.Zero; + + return _torque; + } + + set + { + if (value.IsFinite()) + { + AddChange(changes.Torque, value); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); + } + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get + { + if (givefakeori>0) + return fakeori; + else + + return _orientation; + } + set + { + if (QuaternionIsFinite(value)) + { + fakeori = value; + givefakeori++; + AddChange(changes.Orientation, value); + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); + + } + } + + public override Vector3 Acceleration + { + get { return _acceleration; } + set { } + } + + public override Vector3 RotationalVelocity + { + get + { + Vector3 pv = Vector3.Zero; + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + return pv; + + return m_rotationalVelocity; + } + set + { + if (value.IsFinite()) + { + m_rotationalVelocity = value; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); + } + } + } + + + public override float Buoyancy + { + get { return m_buoyancy; } + set + { + m_buoyancy = value; + } + } + + public override bool FloatOnWater + { + set + { + AddChange(changes.CollidesWater, value); + } + } + + public override Vector3 PIDTarget + { + set + { + if (value.IsFinite()) + { + m_PIDTarget = value; + } + else + m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); + } + } + + public override bool PIDActive { set { m_usePID = value; } } + public override float PIDTau { set { m_PIDTau = value; } } + + public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } + public override bool PIDHoverActive { set { m_useHoverPID = value; } } + public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } + public override float PIDHoverTau { set { m_PIDHoverTau = value; } } + + public override Quaternion APIDTarget { set { return; } } + + public override bool APIDActive { set { return; } } + + public override float APIDStrength { set { return; } } + + public override float APIDDamping { set { return; } } + + public override int VehicleType + { + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + if (m_vehicle == null) + { + if (value != (int)Vehicle.TYPE_NONE) + { + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + else + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + + public override void VehicleFloatParam(int param, float value) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleVectorParam(int param, Vector3 value) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public override void VehicleFlags(int param, bool remove) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(param, remove); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + public void SetAcceleration(Vector3 accel) + { + _acceleration = accel; + } + + public override void AddForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); + } + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(Vector3 force, bool pushforce) + { + if (force.IsFinite()) + { + AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); + } + } + + public override void CrossingFailure() + { + m_crossingfailures++; + changeDisable(false); + } + + public override void SetMomentum(Vector3 momentum) + { + } + + public override void SetMaterial(int pMaterial) + { + m_material = pMaterial; + primContactData.mu = _parent_scene.m_materialContactsData[pMaterial].mu; + primContactData.bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; + } + + public void setPrimForRemoval() + { + AddChange(changes.Remove, null); + } + + public override void link(PhysicsActor obj) + { + AddChange(changes.Link, obj); + } + + public override void delink() + { + AddChange(changes.DeLink, null); + } + + public override void LockAngularMotion(Vector3 axis) + { + // reverse the zero/non zero values for ODE. + if (axis.IsFinite()) + { + axis.X = (axis.X > 0) ? 1f : 0f; + axis.Y = (axis.Y > 0) ? 1f : 0f; + axis.Z = (axis.Z > 0) ? 1f : 0f; + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + AddChange(changes.AngLock, axis); + } + else + { + m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); + } + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.AddCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.RemoveCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) + { + if (CollisionEventsThisFrame == null) + CollisionEventsThisFrame = new CollisionEventUpdate(); + + CollisionEventsThisFrame.AddCollider(CollidedWith, contact); + } + + public void SendCollisions() + { + if (CollisionEventsThisFrame == null) + return; + + base.SendCollisionUpdate(CollisionEventsThisFrame); + + if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) + CollisionEventsThisFrame = null; + else + CollisionEventsThisFrame = new CollisionEventUpdate(); + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, + Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) + { + Name = primName; + + m_vehicle = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); + } + _position = pos; + givefakepos = 0; + + PID_D = parent_scene.bodyPIDD; + PID_G = parent_scene.bodyPIDG; + m_density = parent_scene.geomDefaultDensity; + // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + Body = IntPtr.Zero; + + if (!size.IsFinite()) + { + size = new Vector3(0.5f, 0.5f, 0.5f); + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); + } + + if (size.X <= 0) size.X = 0.01f; + if (size.Y <= 0) size.Y = 0.01f; + if (size.Z <= 0) size.Z = 0.01f; + + _size = size; + + + if (!QuaternionIsFinite(rotation)) + { + rotation = Quaternion.Identity; + m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); + } + + _orientation = rotation; + givefakeori = 0; + + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = IntPtr.Zero; + + if (pos.Z < 0) + { + m_isphysical = false; + } + else + { + m_isphysical = pisPhysical; + } + m_fakeisphysical = m_isphysical; + + m_isVolumeDetect = false; + + m_force = Vector3.Zero; + + m_iscolliding = false; + m_wascolliding = false; + m_colliderfilter = 0; + + hasOOBoffsetFromMesh = false; + _triMeshData = IntPtr.Zero; + + + primContactData.mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; + primContactData.bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; + + CalcPrimBodyData(); + + m_building = true; // control must set this to false when done + + AddChange(changes.Add, null); + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + m_interpenetrationcount = 0; + m_disabled = false; + } + + private void createAMotor(Vector3 axis) + { + if (Body == IntPtr.Zero) + return; + + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); + + if (axisnum <= 0) + return; + + // stop it + d.BodySetTorque(Body, 0, 0, 0); + d.BodySetAngularVel(Body, 0, 0, 0); + + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + + d.JointSetAMotorNumAxes(Amotor, axisnum); + + // get current orientation to lock + + d.Quaternion dcur = d.BodyGetQuaternion(Body); + Quaternion curr; // crap convertion between identical things + curr.X = dcur.X; + curr.Y = dcur.Y; + curr.Z = dcur.Z; + curr.W = dcur.W; + Vector3 ax; + + const int StopERP = 7; + const int StopCFM = 8; + + int i = 0; + int j = 0; + if (axis.X == 0) + { + ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X + // ODE should do this with axis relative to body 1 but seems to fail + d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, (int)StopERP, 0.8f); + i++; + j = 256; // odeplugin.cs doesn't have all parameters so this moves to next axis set + } + + if (axis.Y == 0) + { + ax = (new Vector3(0, 1, 0)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + i++; + j += 256; + } + + if (axis.Z == 0) + { + ax = (new Vector3(0, 0, 1)) * curr; + d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); + d.JointSetAMotorAngle(Amotor, i, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); + d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); + d.JointSetAMotorParam(Amotor, j + (int)StopCFM, 0f); + d.JointSetAMotorParam(Amotor, j + (int)StopERP, 0.8f); + } + } + + private bool setMesh(OdeScene parent_scene) + { + if (Body != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildDelink(this,false); + } + } + else + { + DestroyBody(); + } + } + + IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true); + if (mesh == null) + { + m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); + return false; + } + + IntPtr vertices, indices; + int vertexCount, indexCount; + int vertexStride, triStride; + + mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap + mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage + + if (vertexCount == 0 || indexCount == 0) + { + m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. It can be a sculp with alpha channel in map. Replacing it by a small box.", Name, _position.X, _position.Y, _position.Z); + _size.X = 0.01f; + _size.Y = 0.01f; + _size.Z = 0.01f; + return false; + } + +// primOOBoffset = mesh.GetCentroid(); +// hasOOBoffsetFromMesh = true; + hasOOBoffsetFromMesh = false; + + _triMeshData = d.GeomTriMeshDataCreate(); + + d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); + d.GeomTriMeshDataPreprocess(_triMeshData); + + mesh.releaseSourceMeshData(); + + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null)); + } + + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); + return false; + } + return true; + } + + private void SetGeom(IntPtr geom) + { + prim_geom = geom; + //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + CalcPrimBodyData(); + + _parent_scene.geom_name_map[prim_geom] = Name; + _parent_scene.actor_name_map[prim_geom] = this; + +/* + if (childPrim) + { + if (_parent != null && _parent is OdePrim) + { + OdePrim parent = (OdePrim)_parent; + //Console.WriteLine("SetGeom calls ChildSetGeom"); + parent.ChildSetGeom(this); + } + } + */ + } + else + m_log.Warn("Setting bad Geom"); + } + + + /// + /// Create a geometry for the given mesh in the given target space. + /// + /// + /// If null, then a mesh is used that is based on the profile shape data. + private void CreateGeom() + { + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + + bool haveMesh = false; + hasOOBoffsetFromMesh = false; + + if (_parent_scene.needsMeshing(_pbs)) + { + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + } + + if(!haveMesh) + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 + && _size.X == _size.Y && _size.Y == _size.Z) + { // it's a sphere + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); + return; + } + } + else + {// do it as a box + _parent_scene.waitForSpaceUnlock(m_targetSpace); + try + { + //Console.WriteLine(" CreateGeom 4"); + SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); + } + catch (Exception e) + { + m_log.Warn("[PHYSICS]: Create box failed: {0}", e); + return; + } + } + } + } + + /// + /// Set a new geometry for this prim. + /// + /// + private void RemoveGeom() + { + if (prim_geom != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(prim_geom); + _parent_scene.actor_name_map.Remove(prim_geom); + try + { + d.GeomDestroy(prim_geom); + if (_triMeshData != IntPtr.Zero) + { + d.GeomTriMeshDataDestroy(_triMeshData); + _triMeshData = IntPtr.Zero; + } + } + // catch (System.AccessViolationException) + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name,e); + } + + prim_geom = IntPtr.Zero; + } + else + { + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); + } + Body = IntPtr.Zero; + hasOOBoffsetFromMesh = false; +// CalcPrimBodyData(); + } + + private void ChildSetGeom(OdePrim odePrim) + { + // well.. + DestroyBody(); + MakeBody(); + } + + //sets non physical prim m_targetSpace to right space in spaces grid for static prims + // should only be called for non physical prims unless they are becoming non physical + private void SetInStaticSpace(OdePrim prim) + { + IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); + prim.m_targetSpace = targetSpace; + d.GeomEnable(prim_geom); + } + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + /* + if (m_targetSpace != _parent_scene.ActiveSpace) + { + m_targetSpace = _parent_scene.ActiveSpace; + + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom != IntPtr.Zero) + { + d.SpaceAdd(m_targetSpace, prm.prim_geom); + prm.m_targetSpace = m_targetSpace; + } + } + d.SpaceAdd(m_targetSpace, prim_geom); + } + */ + d.GeomEnable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomEnable(prm.prim_geom); + + d.BodyEnable(Body); + } + } + resetCollisionAccounting(); // this sets m_disable to false + } + + private void disableBodySoft() + { + m_disabled = true; + if (!childPrim) + { + if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) + { + /* + if (m_targetSpace == _parent_scene.ActiveSpace) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.m_targetSpace != IntPtr.Zero && prm.prim_geom != IntPtr.Zero) + { + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + prm.m_targetSpace = IntPtr.Zero; + } + } + d.SpaceRemove(m_targetSpace, prim_geom); + m_targetSpace = IntPtr.Zero; + } + */ + d.GeomDisable(prim_geom); + foreach (OdePrim prm in childrenPrim) + d.GeomDisable(prm.prim_geom); + d.BodyDisable(Body); + } + } + } + + private void MakeBody() + { + if (!m_isphysical) // only physical get bodies + return; + + if (childPrim) // child prims don't get bodies; + return; + + if (m_building) + return; + + if (prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); + return; + } + + if (Body != IntPtr.Zero) + { + d.BodyDestroy(Body); + Body = IntPtr.Zero; + m_log.Warn("[PHYSICS]: MakeBody called having a body"); + } + + + if (d.GeomGetBody(prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); + } + + d.Matrix3 mymat = new d.Matrix3(); + d.Quaternion myrot = new d.Quaternion(); + d.Mass objdmass = new d.Mass { }; + + Body = d.BodyCreate(_parent_scene.world); + + DMassDup(ref primdMass, out objdmass); + + // rotate inertia + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + + // set the body rotation and position + d.BodySetRotation(Body, ref mymat); + + // recompute full object inertia if needed + if (childrenPrim.Count > 0) + { + d.Matrix3 mat = new d.Matrix3(); + d.Quaternion quat = new d.Quaternion(); + d.Mass tmpdmass = new d.Mass { }; + Vector3 rcm; + + rcm.X = _position.X + objdmass.c.X; + rcm.Y = _position.Y + objdmass.c.Y; + rcm.Z = _position.Z + objdmass.c.Z; + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); + continue; + } + + + + DMassCopy(ref prm.primdMass, ref tmpdmass); + + // apply prim current rotation to inertia + quat.X = prm._orientation.X; + quat.Y = prm._orientation.Y; + quat.Z = prm._orientation.Z; + quat.W = prm._orientation.W; + d.RfromQ(out mat, ref quat); + d.MassRotate(ref tmpdmass, ref mat); + + Vector3 ppos = prm._position; + ppos.X += tmpdmass.c.X - rcm.X; + ppos.Y += tmpdmass.c.Y - rcm.Y; + ppos.Z += tmpdmass.c.Z - rcm.Z; + + // refer inertia to root prim center of mass position + d.MassTranslate(ref tmpdmass, + ppos.X, + ppos.Y, + ppos.Z); + + d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia + // fix prim colision cats + + if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) + { + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); + } + + d.GeomClearOffset(prm.prim_geom); + d.GeomSetBody(prm.prim_geom, Body); + prm.Body = Body; + d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation + } + } + } + + d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset + // associate root geom with body + d.GeomSetBody(prim_geom, Body); + + d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); + d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); + + d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body + myrot.W = -myrot.W; + d.RfromQ(out mymat, ref myrot); + d.MassRotate(ref objdmass, ref mymat); + d.BodySetMass(Body, ref objdmass); + _mass = objdmass.mass; + + m_collisionCategories |= CollisionCategories.Body; + m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); + + d.BodySetAutoDisableFlag(Body, true); + d.BodySetAutoDisableSteps(Body, body_autodisable_frames); + // d.BodySetLinearDampingThreshold(Body, 0.01f); + // d.BodySetAngularDampingThreshold(Body, 0.001f); + d.BodySetDamping(Body, .001f, .0002f); + + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + + m_interpenetrationcount = 0; + m_collisionscore = 0; + + m_disabled = false; + + if (m_targetSpace != _parent_scene.ActiveSpace) + { + if (m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(m_targetSpace, prim_geom)) + d.SpaceRemove(m_targetSpace, prim_geom); + } + + m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prim_geom); + } + + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + if (prm.prim_geom == IntPtr.Zero) + continue; + + Vector3 ppos = prm._position; + d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position + + prm.m_collisionCategories |= CollisionCategories.Body; + prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); + d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); + + if (prm.m_targetSpace != _parent_scene.ActiveSpace) + { + if (prm.m_targetSpace != IntPtr.Zero) + { + _parent_scene.waitForSpaceUnlock(m_targetSpace); + if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) + d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); + } + prm.m_targetSpace = _parent_scene.ActiveSpace; + d.SpaceAdd(m_targetSpace, prm.prim_geom); + } + d.GeomEnable(prm.prim_geom); + prm.m_disabled = false; + prm.m_interpenetrationcount = 0; + prm.m_collisionscore = 0; + _parent_scene.addActivePrim(prm); + } + } + + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) + { + createAMotor(m_angularlock); + } + + d.GeomEnable(prim_geom); + m_disabled = false; + _parent_scene.addActivePrim(this); + } + + private void DestroyBody() + { + if (Body != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + UpdateDataFromGeom(); + d.GeomSetBody(prim_geom, IntPtr.Zero); + SetInStaticSpace(this); + } + + if (!childPrim) + { + lock (childrenPrim) + { + foreach (OdePrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.m_collisionCategories &= ~CollisionCategories.Body; + prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + if (prm.prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); + prm.UpdateDataFromGeom(); + SetInStaticSpace(prm); + } + prm.Body = IntPtr.Zero; + prm._mass = prm.primMass; + prm.m_collisionscore = 0; + } + } + d.BodyDestroy(Body); + } + Body = IntPtr.Zero; + } + _mass = primMass; + m_disabled = true; + m_collisionscore = 0; + } + + #region Mass Calculation + + private float CalculatePrimVolume() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + } + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + return volume; + } + + + private void CalcPrimBodyData() + { + float volume; + + if (prim_geom == IntPtr.Zero) + { + // Ubit let's have a initial basic OOB + primOOBsize.X = _size.X; + primOOBsize.Y = _size.Y; + primOOBsize.Z = _size.Z; + primOOBoffset = Vector3.Zero; + } + else + { + d.AABB AABB; + d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom + + primOOBsize.X = (AABB.MaxX - AABB.MinX); + primOOBsize.Y = (AABB.MaxY - AABB.MinY); + primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); + if (!hasOOBoffsetFromMesh) + { + primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; + primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; + primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; + } + } + + // also its own inertia and mass + // keep using basic shape mass for now + volume = CalculatePrimVolume(); + + primMass = m_density * volume; + + if (primMass <= 0) + primMass = 0.0001f;//ckrinke: Mass must be greater then zero. + if (primMass > _parent_scene.maximumMassObject) + primMass = _parent_scene.maximumMassObject; + + _mass = primMass; // just in case + + d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); + + d.MassTranslate(ref primdMass, + primOOBoffset.X, + primOOBoffset.Y, + primOOBoffset.Z); + + primOOBsize *= 0.5f; // let obb size be a corner coords + primOOBradiusSQ = primOOBsize.LengthSquared(); + } + + + #endregion + + + /// + /// Add a child prim to this parent prim. + /// + /// Child prim + // I'm the parent + // prim is the child + public void ParentPrim(OdePrim prim) + { + //Console.WriteLine("ParentPrim " + m_primName); + if (this.m_localID != prim.m_localID) + { + DestroyBody(); // for now we need to rebuil entire object on link change + + lock (childrenPrim) + { + // adopt the prim + if (!childrenPrim.Contains(prim)) + childrenPrim.Add(prim); + + // see if this prim has kids and adopt them also + // should not happen for now + foreach (OdePrim prm in prim.childrenPrim) + { + if (!childrenPrim.Contains(prm)) + { + if (prm.Body != IntPtr.Zero) + { + if (prm.prim_geom != IntPtr.Zero) + d.GeomSetBody(prm.prim_geom, IntPtr.Zero); + if(prm.Body != prim.Body) + prm.DestroyBody(); // don't loose bodies around + prm.Body = IntPtr.Zero; + } + + childrenPrim.Add(prm); + prm._parent = this; + } + } + } + //Remove old children from the prim + prim.childrenPrim.Clear(); + + if (prim.Body != IntPtr.Zero) + { + if (prim.prim_geom != IntPtr.Zero) + d.GeomSetBody(prim.prim_geom, IntPtr.Zero); + prim.DestroyBody(); // don't loose bodies around + prim.Body = IntPtr.Zero; + } + + prim.childPrim = true; + prim._parent = this; + + MakeBody(); // full nasty reconstruction + } + } + + private void UpdateChildsfromgeom() + { + if (childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.UpdateDataFromGeom(); + } + } + + private void UpdateDataFromGeom() + { + if (prim_geom != IntPtr.Zero) + { + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom, out lpos); + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + d.Quaternion qtmp = new d.Quaternion { }; + d.GeomCopyQuaternion(prim_geom, out qtmp); + _orientation.W = qtmp.W; + _orientation.X = qtmp.X; + _orientation.Y = qtmp.Y; + _orientation.Z = qtmp.Z; + } + } + + private void ChildDelink(OdePrim odePrim, bool remakebodies) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) // delinking the root prim + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + if (remakebodies) + newroot.MakeBody(); + } + } + } + + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + // odePrim.UpdateDataFromGeom(); + if (remakebodies) + odePrim.MakeBody(); + } + } + if (remakebodies) + MakeBody(); + } + + protected void ChildRemove(OdePrim odePrim, bool reMakeBody) + { + // Okay, we have a delinked child.. destroy all body and remake + if (odePrim != this && !childrenPrim.Contains(odePrim)) + return; + + DestroyBody(); + + if (odePrim == this) + { + OdePrim newroot = null; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + newroot = childrenPrim[0]; + childrenPrim.RemoveAt(0); + foreach (OdePrim prm in childrenPrim) + { + newroot.childrenPrim.Add(prm); + } + childrenPrim.Clear(); + } + if (newroot != null) + { + newroot.childPrim = false; + newroot._parent = null; + newroot.MakeBody(); + } + } + if (reMakeBody) + MakeBody(); + return; + } + else + { + lock (childrenPrim) + { + childrenPrim.Remove(odePrim); + odePrim.childPrim = false; + odePrim._parent = null; + if (reMakeBody) + odePrim.MakeBody(); + } + } + MakeBody(); + } + + #region changes + + private void changeadd() + { + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; + if (!m_isphysical) + SetInStaticSpace(this); + } + + m_building = false; // REMOVE THIS LATER + + + if (m_isphysical && Body == IntPtr.Zero) + { +/* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + { + */ + MakeBody(); +// } + } + } + + private void changeAngularLock(Vector3 newLock) + { + // do we have a Physical object? + if (Body != IntPtr.Zero) + { + //Check that we have a Parent + //If we have a parent then we're not authorative here + if (_parent == null) + { + if (!newLock.ApproxEquals(Vector3.One, 0f)) + { + createAMotor(newLock); + } + else + { + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + } + } + } + // Store this for later in case we get turned into a separate body + m_angularlock = newLock; + } + + private void changeLink(OdePrim NewParent) + { + if (_parent == null && NewParent != null) + { + NewParent.ParentPrim(this); + } + else if (_parent != null) + { + if (_parent is OdePrim) + { + if (NewParent != _parent) + { + (_parent as OdePrim).ChildDelink(this,false); // for now... + childPrim = false; + + if (NewParent != null) + { + NewParent.ParentPrim(this); + } + } + } + } + _parent = NewParent; + } + + + private void Stop() + { + if(!childPrim) + { + m_force = Vector3.Zero; + m_forceacc = Vector3.Zero; + m_angularForceacc = Vector3.Zero; + _torque = Vector3.Zero; + _velocity = Vector3.Zero; + _acceleration = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; + _target_velocity = Vector3.Zero; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + } + + if (Body != IntPtr.Zero) + { + d.BodySetForce(Body, 0f, 0f, 0f); + d.BodySetTorque(Body, 0f, 0f, 0f); + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetAngularVel(Body, 0f, 0f, 0f); + + } + } + + private void changeSelectedStatus(bool newval) + { + m_isSelected = newval; + Stop(); + + if (newval) + { + m_collisionCategories = CollisionCategories.Selected; + m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + disableBodySoft(); + } + else + { + m_collisionCategories = CollisionCategories.Geom; + + if (m_isphysical) + m_collisionCategories |= CollisionCategories.Body; + + m_collisionFlags = m_default_collisionFlags; + + if (m_collidesLand) + m_collisionFlags |= CollisionCategories.Land; + if (m_collidesWater) + m_collisionFlags |= CollisionCategories.Water; + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + + enableBodySoft(); + } + + resetCollisionAccounting(); + } + + private void changePosition(Vector3 newPos) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _position = newPos; + } + } + else + { + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; +// changeSelectedStatus(); + resetCollisionAccounting(); + } + + private void changeOrientation(Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim) // inertia is messed, must rebuild + { + if (m_building) + { + _orientation = newOri; + } + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + } + } + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + resetCollisionAccounting(); + } + + private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) + { + if (m_isphysical) + { + if (childPrim && m_building) // inertia is messed, must rebuild + { + _position = newPos; + _orientation = newOri; + } + else + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) + createAMotor(m_angularlock); + } + if (_position != newPos) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + else + { + // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + + if (prim_geom != IntPtr.Zero) + { + if (newOri != _orientation) + { + d.Quaternion myrot = new d.Quaternion(); + myrot.X = newOri.X; + myrot.Y = newOri.Y; + myrot.Z = newOri.Z; + myrot.W = newOri.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + _orientation = newOri; + } + + if (newPos != _position) + { + d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + + m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); + } + } + } + givefakepos--; + if (givefakepos < 0) + givefakepos = 0; + givefakeori--; + if (givefakeori < 0) + givefakeori = 0; + + resetCollisionAccounting(); + } + + + private void changeDisable(bool disable) + { + if (disable) + { + if (!m_disabled) + disableBodySoft(); + } + else + { + if (m_disabled) + enableBodySoft(); + } + } + + private void changePhysicsStatus(bool NewStatus) + { + m_isphysical = NewStatus; + + if (!childPrim) + { + if (NewStatus) + { + if (Body == IntPtr.Zero) + { +/* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + { + MakeBody(); + } + } + } + else + { + if (Body != IntPtr.Zero) + { + // UpdateChildsfromgeom(); +/* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + DestroyBody(); + } + } + } + + resetCollisionAccounting(); + } + + private void changeprimsizeshape() + { + OdePrim parent = (OdePrim)_parent; + + bool chp = childPrim; + + if (chp) + { + if (parent != null) + { + parent.DestroyBody(); + } + } + else + { + DestroyBody(); + } + + RemoveGeom(); + + // we don't need to do space calculation because the client sends a position update also. + if (_size.X <= 0) + _size.X = 0.01f; + if (_size.Y <= 0) + _size.Y = 0.01f; + if (_size.Z <= 0) + _size.Z = 0.01f; + // Construction of new prim + + CreateGeom(); + + if (prim_geom != IntPtr.Zero) + { + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + d.Quaternion myrot = new d.Quaternion(); + myrot.X = _orientation.X; + myrot.Y = _orientation.Y; + myrot.Z = _orientation.Z; + myrot.W = _orientation.W; + d.GeomSetQuaternion(prim_geom, ref myrot); + } + + if (chp) + { + if (parent != null) + { + parent.MakeBody(); + } + } + else + MakeBody(); + + resetCollisionAccounting(); + } + + private void changeSize(Vector3 newSize) + { + _size = newSize; + changeprimsizeshape(); + } + + private void changeShape(PrimitiveBaseShape newShape) + { + _pbs = newShape; + changeprimsizeshape(); + } + + private void changeFloatOnWater(bool newval) + { + m_collidesWater = newval; + + if (prim_geom != IntPtr.Zero) + { + if (m_collidesWater) + { + m_collisionFlags |= CollisionCategories.Water; + } + else + { + m_collisionFlags &= ~CollisionCategories.Water; + } + d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); + } + } + + private void changeSetTorque(Vector3 newtorque) + { + if (!m_isSelected) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + } + _torque = newtorque; + } + } + + private void changeForce(Vector3 force) + { + m_force = force; + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeAddForce(Vector3 force) + { + m_forceacc += force; + if (!m_isSelected) + { + lock (this) + { + //m_log.Info("[PHYSICS]: dequeing forcelist"); + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changeAddAngularForce(Vector3 aforce) + { + m_angularForceacc += aforce; + if (!m_isSelected) + { + lock (this) + { + if (m_isphysical && Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + } + m_collisionscore = 0; + m_interpenetrationcount = 0; + } + } + + private void changevelocity(Vector3 newVel) + { + if (!m_isSelected) + { + if (Body != IntPtr.Zero) + { + if (m_disabled) + enableBodySoft(); + else if (!d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + + d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); + } + //resetCollisionAccounting(); + } + _velocity = newVel; + } + + private void changeVolumedetetion(bool newVolDtc) + { + m_isVolumeDetect = newVolDtc; + } + + protected void changeBuilding(bool newbuilding) + { + if ((bool)newbuilding) + { + m_building = true; + DestroyBody(); + } + else + { + m_building = false; + if (!childPrim) + MakeBody(); + } + if (!childPrim && childrenPrim.Count > 0) + { + foreach (OdePrim prm in childrenPrim) + prm.changeBuilding(m_building); // call directly + } + } + + #endregion + + public void Move() + { + if (!childPrim && m_isphysical && Body != IntPtr.Zero && + !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims. + { +// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + float timestep = _parent_scene.ODE_STEPSIZE; + + float fx = 0; + float fy = 0; + float fz = 0; + + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(); + } + else + { + float m_mass = _mass; + + // fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + if (m_usePID) + { + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = + new Vector3( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + else if (m_useHoverPID) + { + //Console.WriteLine("Hover " + Name); + + // If we're using the PID controller, then we have no gravity + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = + new Vector3(0.0f, 0.0f, + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + // ? d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = ((_target_velocity.Z - vel.Z) * (PID_D)); + } + } + else + { + float b = (1.0f - m_buoyancy); + fx = _parent_scene.gravityx * b; + fy = _parent_scene.gravityy * b; + fz = _parent_scene.gravityz * b; + } + + fx *= m_mass; + fy *= m_mass; + fz *= m_mass; + + // constant force + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + fx += m_forceacc.X; + fy += m_forceacc.Y; + fz += m_forceacc.Z; + + m_forceacc = Vector3.Zero; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + d.BodyAddForce(Body, fx, fy, fz); + //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + + Vector3 trq; + + trq = _torque; + trq += m_angularForceacc; + m_angularForceacc = Vector3.Zero; + if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) + { + d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); + } + + } + } + else + { // is not physical, or is not a body or is selected + // _zeroPosition = d.BodyGetPosition(Body); + return; + //Console.WriteLine("Nothing " + Name); + + } + } + + + public void UpdatePositionAndVelocity(float simulatedtime) + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + if (_parent == null && !m_disabled && !m_building) + { + if (Body != IntPtr.Zero) + { + if (m_crossingfailures != 0 && m_crossingfailures < 5) + { + _position.X = Util.Clip(_position.X, 0.4f, _parent_scene.WorldExtents.X - 0.4f); + _position.Y = Util.Clip(_position.Y, 0.4f, _parent_scene.WorldExtents.Y - 0.4f); + _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); + + float tmp = _parent_scene.GetTerrainHeightAtXY(_position.X, _position.Y); + if (_position.Z < tmp) + _position.Z = tmp + 0.2f; + + m_lastposition = _position; + m_lastorientation = _orientation; + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + m_lastVelocity = _velocity; + m_rotationalVelocity = _velocity; + if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) + m_vehicle.Stop(); + + m_crossingfailures = 0; // do this only once + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + enableBodySoft(); + base.RequestPhysicsterseUpdate(); + return; + } + + else if (m_crossingfailures != 0) + { + return; + } + + Vector3 pv = Vector3.Zero; + bool lastZeroFlag = _zeroFlag; + + d.Vector3 lpos; + d.GeomCopyPosition(prim_geom,out lpos); // root position that is seem by rest of simulator + + // we need to use root position since that's all the rest of scene uses + if ( lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X + || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // we can't let it keeping moving and having colisions + // since it can be stucked between something like terrain and edge + // so lets stop and disable it until something else kicks it + if (m_crossingfailures == 0) + { + + _position.X = Util.Clip(lpos.X, -0.5f, _parent_scene.WorldExtents.X + 0.5f); + _position.Y = Util.Clip(lpos.Y, -0.5f, _parent_scene.WorldExtents.Y + 0.5f); + _position.Z = Util.Clip(lpos.Z, -100f, 50000f); + + m_lastposition = _position; + m_lastorientation = _orientation; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); + d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); + disableBodySoft(); // stop collisions + m_crossingfailures++; // do this only once + base.RequestPhysicsterseUpdate(); + return; + } + } + + if (lpos.Z < -100 || lpos.Z > 100000f) + { + lpos.Z = Util.Clip(lpos.Z, -100f, 50000f); + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + d.BodySetLinearVel(Body, 0, 0, 0); // stop it + d.BodySetAngularVel(Body, 0, 0, 0); // stop it + d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere + m_lastposition = _position; + m_lastorientation = _orientation; + + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + + disableBodySoft(); // disable it and colisions + base.RaiseOutOfBounds(_position); + + return; + } + + d.Quaternion ori; + d.GeomCopyQuaternion(prim_geom, out ori); + d.Vector3 vel = d.BodyGetLinearVel(Body); + d.Vector3 rotvel = d.BodyGetAngularVel(Body); + + if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) + && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) + && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) + && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) + && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) + && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) + ) + { + _zeroFlag = true; + //Console.WriteLine("ZFT 2"); + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + m_lastUpdateSent = false; + //m_throttleUpdates = false; + } + + if (_zeroFlag) + { + m_lastposition = _position; + m_lastorientation = _orientation; + + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position.X = lpos.X; + _position.Y = lpos.Y; + _position.Z = lpos.Z; + + _velocity.X = vel.X; + _velocity.Y = vel.Y; + _velocity.Z = vel.Z; + + _orientation.X = ori.X; + _orientation.Y = ori.Y; + _orientation.Z = ori.Z; + _orientation.W = ori.W; + + _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); + + if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) + { + m_rotationalVelocity = pv; + } + else + { + m_rotationalVelocity.X = rotvel.X; + m_rotationalVelocity.Y = rotvel.Y; + m_rotationalVelocity.Z = rotvel.Z; + } + + m_lastUpdateSent = false; + if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + { + m_lastposition = _position; + m_lastorientation = _orientation; + base.RequestPhysicsterseUpdate(); + } + else + { + throttleCounter++; + } + } + } + else if (!m_lastUpdateSent || !_zeroFlag) + { + // Not a body.. so Make sure the client isn't interpolating + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + _zeroFlag = true; + + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + } + } + + internal static bool QuaternionIsFinite(Quaternion q) + { + if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) + return false; + if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) + return false; + if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) + return false; + if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) + return false; + return true; + } + + internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) + { + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + + private static void DMassDup(ref d.Mass src, out d.Mass dst) + { + dst = new d.Mass { }; + + dst.c.W = src.c.W; + dst.c.X = src.c.X; + dst.c.Y = src.c.Y; + dst.c.Z = src.c.Z; + dst.mass = src.mass; + dst.I.M00 = src.I.M00; + dst.I.M01 = src.I.M01; + dst.I.M02 = src.I.M02; + dst.I.M10 = src.I.M10; + dst.I.M11 = src.I.M11; + dst.I.M12 = src.I.M12; + dst.I.M20 = src.I.M20; + dst.I.M21 = src.I.M21; + dst.I.M22 = src.I.M22; + } + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; + } + + // nasty switch + switch (what) + { + case changes.Add: + changeadd(); + break; + case changes.Remove: + //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... + //When we return true, it destroys all of the prims in the linkset anyway + if (_parent != null) + { + OdePrim parent = (OdePrim)_parent; + parent.ChildRemove(this,false); + } + else + ChildRemove(this,false); + + RemoveGeom(); + m_targetSpace = IntPtr.Zero; + if (m_eventsubscription > 0) + UnSubscribeEvents(); + return true; + + case changes.Link: + OdePrim tmp = (OdePrim)arg; + changeLink(tmp); + break; + + case changes.DeLink: + changeLink(null); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changevelocity((Vector3)arg); + break; + +// case changes.Acceleration: +// changeacceleration((Vector3)arg); +// break; +// case changes.AngVelocity: +// changeangvelocity((Vector3)arg); +// break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; + + case changes.Shape: + changeShape((PrimitiveBaseShape) arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool) arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; + + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; + } + return false; + } + + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange(this, what, arg); + } + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs new file mode 100644 index 0000000..4b3f83b --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs @@ -0,0 +1,443 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Text; +using OpenMetaverse; +using OpenSim.Region.Physics.Manager; +using OdeAPI; +using log4net; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// Processes raycast requests as ODE is in a state to be able to do them. + /// This ensures that it's thread safe and there will be no conflicts. + /// Requests get returned by a different thread then they were requested by. + /// + public class ODERayCastRequestManager + { + /// + /// Pending ray requests + /// + protected OpenSim.Framework.LocklessQueue m_PendingRequests = new OpenSim.Framework.LocklessQueue(); + + /// + /// Scene that created this object. + /// + private OdeScene m_scene; + + IntPtr ray; + + private const int ColisionContactGeomsPerTest = 5; + + /// + /// ODE near callback delegate + /// + private d.NearCallback nearCallback; + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private List m_contactResults = new List(); + + public ODERayCastRequestManager(OdeScene pScene) + { + m_scene = pScene; + nearCallback = near; + ray = d.CreateRay(IntPtr.Zero, 1.0f); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.Count = 0; + req.length = length; + req.Normal = direction; + req.Origin = position; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = 0; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Queues a raycast + /// + /// Origin of Ray + /// Ray normal + /// Ray length + /// + /// Return method to send the results + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = IntPtr.Zero; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod) + { + ODERayRequest req = new ODERayRequest(); + req.geom = geom; + req.callbackMethod = retMethod; + req.length = length; + req.Normal = direction; + req.Origin = position; + req.Count = count; + + m_PendingRequests.Enqueue(req); + } + + /// + /// Process all queued raycast requests + /// + /// Time in MS the raycasts took to process. + public int ProcessQueuedRequests() + { + int time = System.Environment.TickCount; + + if (m_PendingRequests.Count <= 0) + return 0; + + if (m_scene.ContactgeomsArray == IntPtr.Zero) // oops something got wrong or scene isn't ready still + { + m_PendingRequests.Clear(); + return 0; + } + + ODERayRequest req; + + int i = 50; // arbitary limit of processed tests per frame + + while(m_PendingRequests.Dequeue(out req)) + { + if (req.geom == IntPtr.Zero) + doSpaceRay(req); + else + doGeomRay(req); + if(--i < 0) + break; + } + + lock (m_contactResults) + m_contactResults.Clear(); + + return System.Environment.TickCount - time; + } + /// + /// Method that actually initiates the raycast with full top space + /// + /// + private void doSpaceRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(m_scene.TopSpace, ray, IntPtr.Zero, nearCallback); + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + /// + /// Method that actually initiates the raycast with a geom + /// + /// + private void doGeomRay(ODERayRequest req) + { + // Create the ray +// IntPtr ray = d.CreateRay(m_scene.TopSpace, req.length); + d.GeomRaySetLength(ray, req.length); + d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); + + // Collide test + d.SpaceCollide2(req.geom, ray, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test + + // Remove Ray +// d.GeomDestroy(ray); + + if (req.callbackMethod == null) + return; + + if (req.callbackMethod is RaycastCallback) + { + // Define default results + bool hitYN = false; + uint hitConsumerID = 0; + float distance = 999999999999f; + Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); + Vector3 snormal = Vector3.Zero; + + // Find closest contact and object. + lock (m_contactResults) + { + foreach (ContactResult cResult in m_contactResults) + { + if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) + { + closestcontact = cResult.Pos; + hitConsumerID = cResult.ConsumerID; + distance = cResult.Depth; + hitYN = true; + snormal = cResult.Normal; + } + } + m_contactResults.Clear(); + } + + ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal); + } + else + { + ((RayCallback)req.callbackMethod)(m_contactResults); + lock (m_PendingRequests) + m_contactResults.Clear(); + } + } + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + IntPtr ContactgeomsArray = m_scene.ContactgeomsArray; + if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + // This is the standard Near. g2 is the ray + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + //Don't test against heightfield Geom, or you'll be sorry! + // Exclude heightfield geom + + if (g1 == IntPtr.Zero || g1 == g2) + return; + + if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) + return; + + // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. + if (d.GeomIsSpace(g1)) + { + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message); + } + return; + } + + int count = 0; + try + { + count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS Ray]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message); + return; + } + + if (count == 0) + return; + + PhysicsActor p1 = null; + + if (g1 != IntPtr.Zero) + m_scene.actor_name_map.TryGetValue(g1, out p1); + + d.ContactGeom curcontact = new d.ContactGeom(); + // Loop over contacts, build results. + for (int i = 0; i < count; i++) + { + if (!GetCurContactGeom(i, ref curcontact)) + break; + if (p1 != null) { + if (p1 is OdePrim) + { + ContactResult collisionresult = new ContactResult(); + + collisionresult.ConsumerID = ((OdePrim)p1).m_localID; + collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z); + collisionresult.Depth = curcontact.depth; + collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y, + curcontact.normal.Z); + lock (m_contactResults) + m_contactResults.Add(collisionresult); + } + } + } + } + + /// + /// Dereference the creator scene so that it can be garbage collected if needed. + /// + internal void Dispose() + { + m_scene = null; + } + } + + public struct ODERayRequest + { + public IntPtr geom; + public Vector3 Origin; + public Vector3 Normal; + public int Count; + public float length; + public object callbackMethod; + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs new file mode 100644 index 0000000..c0c7ff3 --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -0,0 +1,1903 @@ +/* + * based on: + * Ode.NET - .NET bindings for ODE + * Jason Perkins (starkos@industriousone.com) + * Licensed under the New BSD + * Part of the OpenDynamicsEngine +Open Dynamics Engine +Copyright (c) 2001-2007, Russell L. Smith. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions +are met: + +Redistributions of source code must retain the above copyright notice, +this list of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation +and/or other materials provided with the distribution. + +Neither the names of ODE's copyright owner nor the names of its +contributors may be used to endorse or promote products derived from +this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * changes by opensim team; + * changes by Aurora team http://www.aurora-sim.org/ + + * Revision/fixs by Ubit Umarov + */ + +using System; +using System.Runtime.InteropServices; +using System.Security; + +namespace OdeAPI +{ +//#if dDOUBLE +// don't see much use in double precision with time steps of 20ms and 10 iterations used on opensim +// at least we save same memory and memory access time, FPU performance on intel usually is similar +// using dReal = System.Double; +//#else + using dReal = System.Single; +//#endif + + public static class d + { + public static dReal Infinity = dReal.MaxValue; + public static int NTotalBodies = 0; + + #region Flags and Enumerations + + [Flags] + public enum AllocateODEDataFlags : uint + { + BasicData = 0, + CollisionData = 0x00000001, + All = ~0u + } + + [Flags] + public enum IniteODEFlags : uint + { + dInitFlagManualThreadCleanup = 0x00000001 + } + + [Flags] + public enum ContactFlags : int + { + Mu2 = 0x001, + FDir1 = 0x002, + Bounce = 0x004, + SoftERP = 0x008, + SoftCFM = 0x010, + Motion1 = 0x020, + Motion2 = 0x040, + MotionN = 0x080, + Slip1 = 0x100, + Slip2 = 0x200, + Approx0 = 0x0000, + Approx1_1 = 0x1000, + Approx1_2 = 0x2000, + Approx1 = 0x3000 + } + + public enum GeomClassID : int + { + SphereClass, + BoxClass, + CapsuleClass, + CylinderClass, + PlaneClass, + RayClass, + ConvexClass, + GeomTransformClass, + TriMeshClass, + HeightfieldClass, + FirstSpaceClass, + SimpleSpaceClass = FirstSpaceClass, + HashSpaceClass, + QuadTreeSpaceClass, + LastSpaceClass = QuadTreeSpaceClass, + FirstUserClass, + LastUserClass = FirstUserClass + MaxUserClasses - 1, + NumClasses, + MaxUserClasses = 4 + } + + public enum JointType : int + { + None, + Ball, + Hinge, + Slider, + Contact, + Universal, + Hinge2, + Fixed, + Null, + AMotor, + LMotor, + Plane2D + } + + public enum JointParam : int + { + LoStop, + HiStop, + Vel, + FMax, + FudgeFactor, + Bounce, + CFM, + StopERP, + StopCFM, + SuspensionERP, + SuspensionCFM, + LoStop2 = 256, + HiStop2, + Vel2, + FMax2, + FudgeFactor2, + Bounce2, + CFM2, + StopERP2, + StopCFM2, + SuspensionERP2, + SuspensionCFM2, + LoStop3 = 512, + HiStop3, + Vel3, + FMax3, + FudgeFactor3, + Bounce3, + CFM3, + StopERP3, + StopCFM3, + SuspensionERP3, + SuspensionCFM3 + } + + public enum dSweepAndPruneAxis : int + { + XYZ = ((0)|(1<<2)|(2<<4)), + XZY = ((0)|(2<<2)|(1<<4)), + YXZ = ((1)|(0<<2)|(2<<4)), + YZX = ((1)|(2<<2)|(0<<4)), + ZXY = ((2)|(0<<2)|(1<<4)), + ZYX = ((2)|(1<<2)|(0<<4)) + } + + #endregion + + #region Callbacks + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int AABBTestFn(IntPtr o1, IntPtr o2, ref AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int ColliderFn(IntPtr o1, IntPtr o2, int flags, out ContactGeom contact, int skip); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GetAABBFn(IntPtr geom, out AABB aabb); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate ColliderFn GetColliderFnFn(int num); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void GeomDtorFn(IntPtr o); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate dReal HeightfieldGetHeight(IntPtr p_user_data, int x, int z); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate void NearCallback(IntPtr data, IntPtr geom1, IntPtr geom2); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount); + + [UnmanagedFunctionPointer(CallingConvention.Cdecl)] + public delegate int TriRayCallback(IntPtr trimesh, IntPtr ray, int triangleIndex, dReal u, dReal v); + + #endregion + + #region Structs + + [StructLayout(LayoutKind.Sequential)] + public struct AABB + { + public dReal MinX, MaxX; + public dReal MinY, MaxY; + public dReal MinZ, MaxZ; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Contact + { + public SurfaceParameters surface; + public ContactGeom geom; + public Vector3 fdir1; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(Contact)); + } + + + [StructLayout(LayoutKind.Sequential)] + public struct ContactGeom + { + + public Vector3 pos; + public Vector3 normal; + public dReal depth; + public IntPtr g1; + public IntPtr g2; + public int side1; + public int side2; + public static readonly int unmanagedSizeOf = Marshal.SizeOf(typeof(ContactGeom)); + } + + [StructLayout(LayoutKind.Sequential)] + public struct GeomClass + { + public int bytes; + public GetColliderFnFn collider; + public GetAABBFn aabb; + public AABBTestFn aabb_test; + public GeomDtorFn dtor; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct JointFeedback + { + public Vector3 f1; + public Vector3 t1; + public Vector3 f2; + public Vector3 t2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Mass + { + public dReal mass; + public Vector4 c; + public Matrix3 I; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix3 + { + public Matrix3(dReal m00, dReal m10, dReal m20, dReal m01, dReal m11, dReal m21, dReal m02, dReal m12, dReal m22) + { + M00 = m00; M10 = m10; M20 = m20; _m30 = 0.0f; + M01 = m01; M11 = m11; M21 = m21; _m31 = 0.0f; + M02 = m02; M12 = m12; M22 = m22; _m32 = 0.0f; + } + public dReal M00, M10, M20; + private dReal _m30; + public dReal M01, M11, M21; + private dReal _m31; + public dReal M02, M12, M22; + private dReal _m32; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Matrix4 + { + public Matrix4(dReal m00, dReal m10, dReal m20, dReal m30, + dReal m01, dReal m11, dReal m21, dReal m31, + dReal m02, dReal m12, dReal m22, dReal m32, + dReal m03, dReal m13, dReal m23, dReal m33) + { + M00 = m00; M10 = m10; M20 = m20; M30 = m30; + M01 = m01; M11 = m11; M21 = m21; M31 = m31; + M02 = m02; M12 = m12; M22 = m22; M32 = m32; + M03 = m03; M13 = m13; M23 = m23; M33 = m33; + } + public dReal M00, M10, M20, M30; + public dReal M01, M11, M21, M31; + public dReal M02, M12, M22, M32; + public dReal M03, M13, M23, M33; + } + + [StructLayout(LayoutKind.Sequential)] + public struct Quaternion + { + public dReal W, X, Y, Z; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct SurfaceParameters + { + public ContactFlags mode; + public dReal mu; + public dReal mu2; + public dReal bounce; + public dReal bounce_vel; + public dReal soft_erp; + public dReal soft_cfm; + public dReal motion1; + public dReal motion2; + public dReal motionN; + public dReal slip1; + public dReal slip2; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector3 + { + public Vector3(dReal x, dReal y, dReal z) + { + X = x; Y = y; Z = z; _w = 0.0f; + } + public dReal X, Y, Z; + private dReal _w; + } + + + [StructLayout(LayoutKind.Sequential)] + public struct Vector4 + { + public Vector4(dReal x, dReal y, dReal z, dReal w) + { + X = x; Y = y; Z = z; W = w; + } + public dReal X, Y, Z, W; + } + + #endregion + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAllocateODEDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern int AllocateODEDataForThread(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnected"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnected(IntPtr b1, IntPtr b2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dAreConnectedExcluding"), SuppressUnmanagedCodeSecurity] + public static extern bool AreConnectedExcluding(IntPtr b1, IntPtr b2, JointType joint_type); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForce(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelForceAtRelPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelForceAtRelPos(IntPtr body, dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddRelTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddRelTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyAddTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodyAddTorque(IntPtr body, dReal fx, dReal fy, dReal fz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out Vector3 pos); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyPosition(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyQuaternion(IntPtr body, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCopyRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodyCopyRotation(IntPtr body, out dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyiCreate(IntPtr world); + public static IntPtr BodyCreate(IntPtr world) + { + NTotalBodies++; + return BodyiCreate(world); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void BodyiDestroy(IntPtr body); + public static void BodyDestroy(IntPtr body) + { + NTotalBodies--; + BodyiDestroy(body); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyDisable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyDisable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyEnable"), SuppressUnmanagedCodeSecurity] + public static extern void BodyEnable(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableAngularThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetAutoDisableFlag(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableLinearThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetAutoDisableSteps(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAutoDisableTime(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetAngularVelUnsafe(IntPtr body); + public static Vector3 BodyGetAngularVel(IntPtr body) + { + unsafe { return *(BodyGetAngularVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetData(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetFiniteRotationMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetFiniteRotationAxis(IntPtr body, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetForce"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetForceUnsafe(IntPtr body); + public static Vector3 BodyGetForce(IntPtr body) + { + unsafe { return *(BodyGetForceUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyGetGravityMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetGyroscopicMode(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetJoint(IntPtr body, int index); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearVel"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetLinearVelUnsafe(IntPtr body); + public static Vector3 BodyGetLinearVel(IntPtr body) + { + unsafe { return *(BodyGetLinearVelUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetMass(IntPtr body, out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNumJoints"), SuppressUnmanagedCodeSecurity] + public static extern int BodyGetNumJoints(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetPositionUnsafe(IntPtr body); + public static Vector3 BodyGetPosition(IntPtr body) + { + unsafe { return *(BodyGetPositionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetPosRelPoint"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetPosRelPoint(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetQuaternion"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Quaternion* BodyGetQuaternionUnsafe(IntPtr body); + public static Quaternion BodyGetQuaternion(IntPtr body) + { + unsafe { return *(BodyGetQuaternionUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointPos"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointPos(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRelPointVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodyGetRelPointVel(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* BodyGetRotationUnsafe(IntPtr body); + public static Matrix3 BodyGetRotation(IntPtr body) + { + unsafe { return *(BodyGetRotationUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetTorque"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* BodyGetTorqueUnsafe(IntPtr body); + public static Vector3 BodyGetTorque(IntPtr body) + { + unsafe { return *(BodyGetTorqueUnsafe(body)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetWorld"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetWorld(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetFirstGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr BodyGetFirstGeom(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetNextGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr dBodyGetNextGeom(IntPtr Geom); + + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool BodyIsEnabled(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableAngularThreshold(IntPtr body, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableDefaults"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableDefaults(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableFlag(IntPtr body, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableLinearThreshold(IntPtr body, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableSteps(IntPtr body, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAutoDisableTime(IntPtr body, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetData"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetData(IntPtr body, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationMode(IntPtr body, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetFiniteRotationAxis"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetFiniteRotationAxis(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDamping(IntPtr body, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDamping(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetDamping(IntPtr body, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetAngularDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearDampingThreshold(IntPtr body, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetLinearDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal BodyGetAngularDampingThreshold(IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetForce"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetForce(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGravityMode"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetGravityMode(IntPtr body, bool mode); + + /// + /// Sets the Gyroscopic term status on the body specified. + /// + /// Pointer to body + /// NonZero enabled, Zero disabled + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetGyroscopicMode"), SuppressUnmanagedCodeSecurity] + public static extern void dBodySetGyroscopicMode(IntPtr body, int enabled); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetLinearVel"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetLinearVel(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetMass"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetMass(IntPtr body, ref Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetPosition(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetQuaternion(IntPtr body, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetRotation(IntPtr body, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodySetTorque"), SuppressUnmanagedCodeSecurity] + public static extern void BodySetTorque(IntPtr body, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorFromWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorFromWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyVectorToWorld"), SuppressUnmanagedCodeSecurity] + public static extern void BodyVectorToWorld(IntPtr body, dReal px, dReal py, dReal pz, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxBox(ref Vector3 p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 p2, + ref Matrix3 R2, ref Vector3 side2, + ref Vector3 normal, out dReal depth, out int return_code, + int maxc, out ContactGeom contact, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBoxTouchesBox"), SuppressUnmanagedCodeSecurity] + public static extern void BoxTouchesBox(ref Vector3 _p1, ref Matrix3 R1, + ref Vector3 side1, ref Vector3 _p2, + ref Matrix3 R2, ref Vector3 side2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCleanupODEAllDataForThread"), SuppressUnmanagedCodeSecurity] + public static extern void CleanupODEAllDataForThread(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dClosestLineSegmentPoints"), SuppressUnmanagedCodeSecurity] + public static extern void ClosestLineSegmentPoints(ref Vector3 a1, ref Vector3 a2, + ref Vector3 b1, ref Vector3 b2, + ref Vector3 cp1, ref Vector3 cp2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCloseODE"), SuppressUnmanagedCodeSecurity] + public static extern void CloseODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int Collide(IntPtr o1, IntPtr o2, int flags, [In, Out] ContactGeom[] contact, int skip); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCollide"), SuppressUnmanagedCodeSecurity] + public static extern int CollidePtr(IntPtr o1, IntPtr o2, int flags, IntPtr contactgeomarray, int skip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dConnectingJoint"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr ConnectingJoint(IntPtr j1, IntPtr j2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateBox"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateGeom(int 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 CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateRay(IntPtr space, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateSphere(IntPtr space, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback 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 GeomDestroy(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomDisable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomEnable"), SuppressUnmanagedCodeSecurity] + public static extern void GeomEnable(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetAABB"), SuppressUnmanagedCodeSecurity] + public static extern void GeomGetAABB(IntPtr geom, out dReal minX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetBody(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCategoryBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClassData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetClassData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern int GeomGetCollideBits(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetClass"), SuppressUnmanagedCodeSecurity] + public static extern GeomClassID GeomGetClass(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetData(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetOffsetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetOffsetPosition(IntPtr geom) + { + unsafe { return *(GeomGetOffsetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetOffsetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetOffsetRotation(IntPtr geom) + { + unsafe { return *(GeomGetOffsetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetPosition"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Vector3* GeomGetPositionUnsafe(IntPtr geom); + public static Vector3 GeomGetPosition(IntPtr geom) + { + unsafe { return *(GeomGetPositionUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomCopyQuaternion(IntPtr geom, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetRotation"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix3* GeomGetRotationUnsafe(IntPtr geom); + public static Matrix3 GeomGetRotation(IntPtr geom) + { + unsafe { return *(GeomGetRotationUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomGetSpace"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomGetSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, byte[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildByte"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildByte(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildCallback(IntPtr d, IntPtr pUserData, HeightfieldGetHeight pCallback, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, ushort[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, short[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildShort"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildShort(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, float[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildSingle(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, double[] pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataBuildDouble(IntPtr d, IntPtr pHeightData, int bCopyHeightData, + dReal width, dReal depth, int widthSamples, int depthSamples, + dReal scale, dReal offset, dReal thickness, int bWrap); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldDataSetBounds"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldDataSetBounds(IntPtr d, dReal minHeight, dReal maxHeight); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldGetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomHeightfieldGetHeightfieldData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomHeightfieldSetHeightfieldData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomHeightfieldSetHeightfieldData(IntPtr g, IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsEnabled(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsOffset"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsOffset(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomIsSpace"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomIsSpace(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref Vector4 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneGetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneGetParams(IntPtr geom, ref dReal A); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlanePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomPlanePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomPlaneSetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomPlaneSetParams(IntPtr plane, dReal a, dReal b, dReal c, dReal d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref Vector3 start, ref Vector3 dir); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRayGet(IntPtr ray, ref dReal startX, ref dReal dirX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern int GeomRayGetClosestHit(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetLength"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetLength(IntPtr ray); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRayGetParams"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomRayGetParams(IntPtr g, out int firstContact, out int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySet(IntPtr ray, dReal px, dReal py, dReal pz, dReal dx, dReal dy, dReal dz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetClosestHit"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetClosestHit(IntPtr ray, int closestHit); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetLength"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetLength(IntPtr ray, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomRaySetParams"), SuppressUnmanagedCodeSecurity] + public static extern void GeomRaySetParams(IntPtr ray, int firstContact, int backfaceCull); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetBody"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetBody(IntPtr geom, IntPtr body); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCategoryBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCategoryBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetCollideBits"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetCollideBits(IntPtr geom, int bits); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetConvex"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomSetConvex(IntPtr geom, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetData(IntPtr geom, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref Quaternion Q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldQuaternion(IntPtr geom, ref dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetOffsetWorldRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetOffsetWorldRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetPosition"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetPosition(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref Quaternion quat); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetQuaternion"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetQuaternion(IntPtr geom, ref dReal w); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSetRotation"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSetRotation(IntPtr geom, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereGetRadius"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSphereGetRadius(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSpherePointDepth"), SuppressUnmanagedCodeSecurity] + public static extern dReal GeomSpherePointDepth(IntPtr geom, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomSphereSetRadius"), SuppressUnmanagedCodeSecurity] + public static extern void GeomSphereSetRadius(IntPtr geom, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetCleanup(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTransformGetGeom(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformGetInfo"), SuppressUnmanagedCodeSecurity] + public static extern int GeomTransformGetInfo(IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetCleanup(IntPtr geom, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetGeom"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetGeom(IntPtr geom, IntPtr obj); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTransformSetInfo"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTransformSetInfo(IntPtr geom, int info); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble1(IntPtr d, + double[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + double[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildDouble1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildDouble(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + dReal[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + dReal[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSimple1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSingle1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + float[] vertices, int vertexStride, int vertexCount, + int[] indices, int indexCount, int triStride, + float[] normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataBuildSingle1"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataBuildSimple1(IntPtr d, + IntPtr vertices, int vertexStride, int vertexCount, + IntPtr indices, int indexCount, int triStride, + IntPtr normals); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshClearTCCache"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshClearTCCache(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataDestroy(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataGet"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshDataGet(IntPtr d, int data_id); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataPreprocess"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataPreprocess(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataSet"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataSet(IntPtr d, int data_id, IntPtr in_data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshDataUpdate"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshDataUpdate(IntPtr d); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshEnableTC"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshEnableTC(IntPtr g, int geomClass, bool enable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriArrayCallback GeomTriMeshGetArrayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriCallback GeomTriMeshGetCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetData(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetLastTransform"), SuppressUnmanagedCodeSecurity] + public extern unsafe static Matrix4* GeomTriMeshGetLastTransformUnsafe(IntPtr geom); + public static Matrix4 GeomTriMeshGetLastTransform(IntPtr geom) + { + unsafe { return *(GeomTriMeshGetLastTransformUnsafe(geom)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetPoint"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetPoint(IntPtr g, int index, dReal u, dReal v, ref Vector3 outVec); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern TriRayCallback GeomTriMeshGetRayCallback(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangle"), SuppressUnmanagedCodeSecurity] + public extern static void GeomTriMeshGetTriangle(IntPtr g, int index, ref Vector3 v0, ref Vector3 v1, ref Vector3 v2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriangleCount"), SuppressUnmanagedCodeSecurity] + public extern static int GeomTriMeshGetTriangleCount(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshGetTriMeshDataID"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr GeomTriMeshGetTriMeshDataID(IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshIsTCEnabled"), SuppressUnmanagedCodeSecurity] + public static extern bool GeomTriMeshIsTCEnabled(IntPtr g, int geomClass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetArrayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetArrayCallback(IntPtr g, TriArrayCallback arrayCallback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetCallback(IntPtr g, TriCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetData"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetData(IntPtr g, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref Matrix4 last_trans); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetLastTransform"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetLastTransform(IntPtr g, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomTriMeshSetRayCallback"), SuppressUnmanagedCodeSecurity] + public static extern void GeomTriMeshSetRayCallback(IntPtr g, TriRayCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGetConfiguration"), SuppressUnmanagedCodeSecurity] + public static extern string GetConfiguration(string str); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr HashSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceGetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceGetLevels(IntPtr space, out int minlevel, out int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dHashSpaceSetLevels"), SuppressUnmanagedCodeSecurity] + public static extern void HashSpaceSetLevels(IntPtr space, int minlevel, int maxlevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInfiniteAABB"), SuppressUnmanagedCodeSecurity] + public static extern void InfiniteAABB(IntPtr geom, out AABB aabb); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE"), SuppressUnmanagedCodeSecurity] + public static extern void InitODE(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInitODE2"), SuppressUnmanagedCodeSecurity] + public static extern int InitODE2(uint ODEInitFlags); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dIsPositiveDefinite"), SuppressUnmanagedCodeSecurity] + public static extern int IsPositiveDefinite(ref dReal A, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dInvertPDMatrix"), SuppressUnmanagedCodeSecurity] + public static extern int InvertPDMatrix(ref dReal A, out dReal Ainv, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddAMotorTorques"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddAMotorTorques(IntPtr joint, dReal torque1, dReal torque2, dReal torque3); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHingeTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHingeTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddHinge2Torque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddHinge2Torques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddPRTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddPRTorque(IntPtr joint, dReal torque); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddUniversalTorque"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddUniversalTorques(IntPtr joint, dReal torque1, dReal torque2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAddSliderForce"), SuppressUnmanagedCodeSecurity] + public static extern void JointAddSliderForce(IntPtr joint, dReal force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointAttach"), SuppressUnmanagedCodeSecurity] + public static extern void JointAttach(IntPtr joint, IntPtr body1, IntPtr body2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateAMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateAMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateBall"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateBall(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContact(IntPtr world, IntPtr group, ref Contact contact); + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateContact"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateContactPtr(IntPtr world, IntPtr group, IntPtr contact); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateFixed"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateFixed(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateHinge2"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateHinge2(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateLMotor"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateLMotor(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateNull"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateNull(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePR"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePR(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreatePlane2D"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreatePlane2D(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateSlider"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateSlider(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointCreateUniversal"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointCreateUniversal(IntPtr world, IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointDestroy(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngle(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorAngleRate(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetAMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorAxisRel"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorAxisRel(IntPtr j, int anum); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorMode(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetAMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetAMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetBallAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetBody"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetBody(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetData"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGetData(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetFeedback"), SuppressUnmanagedCodeSecurity] + public extern unsafe static JointFeedback* JointGetFeedbackUnsafe(IntPtr j); + public static JointFeedback JointGetFeedback(IntPtr j) + { + unsafe { return *(JointGetFeedbackUnsafe(j)); } + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngle"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngle(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAngleRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeAngleRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHingeParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Angle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Angle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHingeAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHingeAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Anchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Anchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetHinge2Axis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetHinge2Param(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetLMotorAxis(IntPtr j, int anum, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern int JointGetLMotorNumAxes(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetLMotorParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetPRAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetPRPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetPRPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetSliderAxis(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPosition"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPosition(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetSliderPositionRate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetSliderPositionRate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetType"), SuppressUnmanagedCodeSecurity] + public static extern JointType JointGetType(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAnchor2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle1Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle1Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngle2Rate"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalAngle2Rate(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAngles"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAngles(IntPtr j, out dReal angle1, out dReal angle2); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis1(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointGetUniversalAxis2(IntPtr j, out Vector3 result); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern dReal JointGetUniversalParam(IntPtr j, int parameter); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr JointGroupCreate(int max_size); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupDestroy(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointGroupEmpty"), SuppressUnmanagedCodeSecurity] + public static extern void JointGroupEmpty(IntPtr group); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAngle"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAngle(IntPtr j, int anum, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorMode"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorMode(IntPtr j, int mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorNumAxes(IntPtr group, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetAMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetAMotorParam(IntPtr group, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetBallAnchor2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetBallAnchor2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetData"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetData(IntPtr j, IntPtr data); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFeedback"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFeedback(IntPtr j, out JointFeedback feedback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetFixed"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetFixed(IntPtr j); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAnchorDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAnchorDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHingeParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHingeParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Anchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Anchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Axis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Axis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetHinge2Param"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetHinge2Param(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorAxis(IntPtr j, int anum, int rel, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorNumAxes"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorNumAxes(IntPtr j, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetLMotorParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetLMotorParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DAngleParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DAngleParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DXParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DXParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPlane2DYParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPlane2DYParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetPRParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetPRParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxis"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxis(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderAxisDelta"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderAxisDelta(IntPtr j, dReal x, dReal y, dReal z, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetSliderParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetSliderParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAnchor"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAnchor(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis1"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis1(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalAxis2"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalAxis2(IntPtr j, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dJointSetUniversalParam"), SuppressUnmanagedCodeSecurity] + public static extern void JointSetUniversalParam(IntPtr j, int parameter, dReal value); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dLDLTAddTL"), SuppressUnmanagedCodeSecurity] + public static extern void LDLTAddTL(ref dReal L, ref dReal d, ref dReal a, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdd"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdd(ref Mass a, ref Mass b); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassAdjust"), SuppressUnmanagedCodeSecurity] + public static extern void MassAdjust(ref Mass m, dReal newmass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassCheck"), SuppressUnmanagedCodeSecurity] + public static extern bool MassCheck(ref Mass m); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassRotate"), SuppressUnmanagedCodeSecurity] + public static extern void MassRotate(ref Mass mass, ref dReal M00); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBox"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBox(out Mass mass, dReal density, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetBoxTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetBoxTotal(out Mass mass, dReal total_mass, dReal lx, dReal ly, dReal lz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsule"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsule(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCapsuleTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCapsuleTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinder"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinder(out Mass mass, dReal density, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetCylinderTotal"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetCylinderTotal(out Mass mass, dReal total_mass, int direction, dReal radius, dReal length); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetParameters"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetParameters(out Mass mass, dReal themass, + dReal cgx, dReal cgy, dReal cgz, + dReal i11, dReal i22, dReal i33, + dReal i12, dReal i13, dReal i23); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphere"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetSphere(out Mass mass, dReal density, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetSphereTotal"), SuppressUnmanagedCodeSecurity] + public static extern void dMassSetSphereTotal(out Mass mass, dReal total_mass, dReal radius); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetTrimesh"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetTrimesh(out Mass mass, dReal density, IntPtr g); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void MassSetZero(out Mass mass); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMassTranslate"), SuppressUnmanagedCodeSecurity] + public static extern void MassTranslate(ref Mass mass, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply0(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply0"), SuppressUnmanagedCodeSecurity] + private static extern void MultiplyiM3V3(out Vector3 vout, ref Matrix3 matrix, ref Vector3 vect,int p, int q, int r); + public static void MultiplyM3V3(out Vector3 outvector, ref Matrix3 matrix, ref Vector3 invector) + { + MultiplyiM3V3(out outvector, ref matrix, ref invector, 3, 3, 1); + } + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply1(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void Multiply2(out dReal A00, ref dReal B00, ref dReal C00, int p, int q, int r); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void QFromAxisAndAngle(out Quaternion q, dReal ax, dReal ay, dReal az, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQfromR"), SuppressUnmanagedCodeSecurity] + public static extern void QfromR(out Quaternion q, ref Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply0"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply0(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply1"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply1(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply2"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply2(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQMultiply3"), SuppressUnmanagedCodeSecurity] + public static extern void QMultiply3(out Quaternion qa, ref Quaternion qb, ref Quaternion qc); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void QSetIdentity(out Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref Vector3 center, ref Vector3 extents, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dQuadTreeSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr QuadTreeSpaceCreate(IntPtr space, ref dReal centerX, ref dReal extentsX, int depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRandReal"), SuppressUnmanagedCodeSecurity] + public static extern dReal RandReal(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFrom2Axes"), SuppressUnmanagedCodeSecurity] + public static extern void RFrom2Axes(out Matrix3 R, dReal ax, dReal ay, dReal az, dReal bx, dReal by, dReal bz); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromAxisAndAngle"), SuppressUnmanagedCodeSecurity] + public static extern void RFromAxisAndAngle(out Matrix3 R, dReal x, dReal y, dReal z, dReal angle); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromEulerAngles"), SuppressUnmanagedCodeSecurity] + public static extern void RFromEulerAngles(out Matrix3 R, dReal phi, dReal theta, dReal psi); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRfromQ"), SuppressUnmanagedCodeSecurity] + public static extern void RfromQ(out Matrix3 R, ref Quaternion q); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRFromZAxis"), SuppressUnmanagedCodeSecurity] + public static extern void RFromZAxis(out Matrix3 R, dReal ax, dReal ay, dReal az); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dRSetIdentity"), SuppressUnmanagedCodeSecurity] + public static extern void RSetIdentity(out Matrix3 R); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetValue"), SuppressUnmanagedCodeSecurity] + public static extern void SetValue(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSetZero"), SuppressUnmanagedCodeSecurity] + public static extern void SetZero(out dReal a, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSimpleSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SimpleSpaceCreate(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveCholesky"), SuppressUnmanagedCodeSecurity] + public static extern void SolveCholesky(ref dReal L, out dReal b, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveL1T"), SuppressUnmanagedCodeSecurity] + public static extern void SolveL1T(ref dReal L, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSolveLDLT"), SuppressUnmanagedCodeSecurity] + public static extern void SolveLDLT(ref dReal L, ref dReal d, out dReal b, int n, int nskip); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceAdd"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceAdd(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceLockQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceLockQuery(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceClean"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceClean(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide(IntPtr space, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceCollide2"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceCollide2(IntPtr space1, IntPtr space2, IntPtr data, NearCallback callback); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceDestroy(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceGetCleanup(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetNumGeoms"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetNumGeoms(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetGeom"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SpaceGetGeom(IntPtr space, int i); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceGetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern int SpaceGetSublevel(IntPtr space); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceQuery"), SuppressUnmanagedCodeSecurity] + public static extern bool SpaceQuery(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceRemove"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceRemove(IntPtr space, IntPtr geom); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetCleanup"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetCleanup(IntPtr space, bool mode); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSpaceSetSublevel"), SuppressUnmanagedCodeSecurity] + public static extern void SpaceSetSublevel(IntPtr space, int sublevel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dSweepAndPruneSpaceCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr SweepAndPruneSpaceCreate(IntPtr space, int AxisOrder); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dVectorScale"), SuppressUnmanagedCodeSecurity] + public static extern void VectorScale(out dReal a, ref dReal d, int n); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldCreate"), SuppressUnmanagedCodeSecurity] + public static extern IntPtr WorldCreate(); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldDestroy"), SuppressUnmanagedCodeSecurity] + public static extern void WorldDestroy(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableAverageSamplesCount(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableAngularThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern bool WorldGetAutoDisableFlag(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableLinearThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoDisableSteps(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAutoDisableTime(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetAutoEnableDepthSF1(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetCFM"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetCFM(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetERP"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetERP(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out Vector3 gravity); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldGetGravity(IntPtr world, out dReal X); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactMaxCorrectingVel(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetContactSurfaceLayer(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetAngularDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDamping(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetLinearDampingThreshold(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern int WorldGetQuickStepNumIterations(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetQuickStepW(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldGetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern dReal WorldGetMaxAngularSpeed(IntPtr world); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out Vector3 force); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldImpulseToForce"), SuppressUnmanagedCodeSecurity] + public static extern void WorldImpulseToForce(IntPtr world, dReal stepsize, dReal ix, dReal iy, dReal iz, out dReal forceX); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldQuickStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldQuickStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAngularDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAngularDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAngularThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAngularThreshold(IntPtr world, dReal angular_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableAverageSamplesCount"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableAverageSamplesCount(IntPtr world, int average_samples_count); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableFlag"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableFlag(IntPtr world, bool do_auto_disable); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableLinearThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableLinearThreshold(IntPtr world, dReal linear_threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableSteps"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableSteps(IntPtr world, int steps); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoDisableTime"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoDisableTime(IntPtr world, dReal time); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetAutoEnableDepthSF1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetAutoEnableDepthSF1(IntPtr world, int autoEnableDepth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetCFM"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetCFM(IntPtr world, dReal cfm); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactMaxCorrectingVel"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactMaxCorrectingVel(IntPtr world, dReal vel); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetContactSurfaceLayer"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetContactSurfaceLayer(IntPtr world, dReal depth); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetDamping(IntPtr world, dReal linear_scale, dReal angular_scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetERP"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetERP(IntPtr world, dReal erp); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetGravity"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetGravity(IntPtr world, dReal x, dReal y, dReal z); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDamping"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDamping(IntPtr world, dReal scale); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetLinearDampingThreshold"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetLinearDampingThreshold(IntPtr world, dReal threshold); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepNumIterations"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepNumIterations(IntPtr world, int num); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetQuickStepW"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetQuickStepW(IntPtr world, dReal over_relaxation); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldSetMaxAngularSpeed"), SuppressUnmanagedCodeSecurity] + public static extern void WorldSetMaxAngularSpeed(IntPtr world, dReal max_speed); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStep"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStep(IntPtr world, dReal stepsize); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldStepFast1"), SuppressUnmanagedCodeSecurity] + public static extern void WorldStepFast1(IntPtr world, dReal stepsize, int maxiterations); + + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dWorldExportDIF"), SuppressUnmanagedCodeSecurity] + public static extern void WorldExportDIF(IntPtr world, string filename, bool append, string prefix); + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs new file mode 100644 index 0000000..215d47a --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdePlugin.cs @@ -0,0 +1,86 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + /// + /// ODE plugin + /// + public class OdePlugin : IPhysicsPlugin + { + //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + + private OdeScene m_scene; + + public bool Init() + { + if (d.InitODE2(0) != 0) + { + if (d.AllocateODEDataForThread(~0U) == 0) + { + d.CloseODE(); + return false; + } + return true; + } + return false; + } + + public PhysicsScene GetScene(String sceneIdentifier) + { + if (m_scene == null) + { + m_scene = new OdeScene(sceneIdentifier); + } + return (m_scene); + } + + public string GetName() + { + return ("UbitODE"); + } + + public void Dispose() + { + d.CloseODE(); + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs new file mode 100644 index 0000000..74de2ee --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -0,0 +1,2741 @@ +/* + * 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 USE_DRAWSTUFF +//#define SPAM + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using System.Threading; +using System.IO; +using System.Diagnostics; +using log4net; +using Nini.Config; +using OdeAPI; +#if USE_DRAWSTUFF +using ODEDrawstuff; +#endif +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public enum StatusIndicators : int + { + Generic = 0, + Start = 1, + End = 2 + } + + public struct sCollisionData + { + public uint ColliderLocalId; + public uint CollidedWithLocalId; + public int NumberOfCollisions; + public int CollisionType; + public int StatusIndicator; + public int lastframe; + } + + [Flags] + public enum CollisionCategories : int + { + Disabled = 0, + Geom = 0x00000001, + Body = 0x00000002, + Space = 0x00000004, + Character = 0x00000008, + Land = 0x00000010, + Water = 0x00000020, + Wind = 0x00000040, + Sensor = 0x00000080, + Selected = 0x00000100 + } + + /// + /// Material type for a primitive + /// + public enum Material : int + { + /// + Stone = 0, + /// + Metal = 1, + /// + Glass = 2, + /// + Wood = 3, + /// + Flesh = 4, + /// + Plastic = 5, + /// + Rubber = 6, + + light = 7 // compatibility with old viewers + } + + public enum changes : int + { + Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) + Remove, + Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root + // or removes from a object if arg is null + DeLink, + Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child + Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child + PosOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + OriOffset, // not in use + // arg Vector3 new position in local coords. Changes prim position in object + Velocity, + AngVelocity, + Acceleration, + Force, + Torque, + + AddForce, + AddAngForce, + AngLock, + + Size, + Shape, + + CollidesWater, + VolumeDtc, + + Physical, + Selected, + disabled, + building, + + Null //keep this last used do dim the methods array. does nothing but pulsing the prim + } + + public struct ODEchangeitem + { + public OdePrim prim; + public OdeCharacter character; + public changes what; + public Object arg; + } + + public class OdeScene : PhysicsScene + { + private readonly ILog m_log; + // private Dictionary m_storedCollisions = new Dictionary(); + + private int threadid = 0; + private Random fluidRandomizer = new Random(Environment.TickCount); + + const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; + const float comumContactERP = 0.6f; + const float comumSoftContactERP = 0.1f; + const float comumContactCFM = 0.0001f; + + float frictionScale = 1.0f; + + float frictionMovementMult = 0.3f; + + float TerrainBounce = 0.3f; + float TerrainFriction = 0.3f; + + public float AvatarBounce = 0.3f; + public float AvatarFriction = 0;// 0.9f * 0.5f; + + private const uint m_regionWidth = Constants.RegionSize; + private const uint m_regionHeight = Constants.RegionSize; + + public float ODE_STEPSIZE = 0.020f; + private float metersInSpace = 25.6f; + private float m_timeDilation = 1.0f; + + public float gravityx = 0f; + public float gravityy = 0f; + public float gravityz = -9.8f; + + + private float waterlevel = 0f; + private int framecount = 0; + + internal IntPtr WaterGeom; + + public float avPIDD = 3200f; // make it visible + public float avPIDP = 1400f; // make it visible + private float avCapRadius = 0.37f; + private float avDensity = 3f; + private float avMovementDivisorWalk = 1.3f; + private float avMovementDivisorRun = 0.8f; + private float minimumGroundFlightOffset = 3f; + public float maximumMassObject = 10000.01f; + + public bool meshSculptedPrim = true; + public bool forceSimplePrimMeshing = false; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; + + public float geomDefaultDensity = 10.000006836f; + + public int geomContactPointsStartthrottle = 3; + public int geomUpdatesPerThrottledUpdate = 15; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + + public int geomCrossingFailuresBeforeOutofbounds = 6; + + public int bodyFramesAutoDisable = 20; + + private float[] _watermap; + private bool m_filterCollisions = true; + + private d.NearCallback nearCallback; + + private readonly HashSet _characters = new HashSet(); + private readonly HashSet _prims = new HashSet(); + private readonly HashSet _activeprims = new HashSet(); + + private readonly Object _taintedCharacterLock = new Object(); + private readonly HashSet _taintedCharacterH = new HashSet(); // faster verification of repeated character taints + private readonly Queue _taintedCharacterQ = new Queue(); // character taints + + public OpenSim.Framework.LocklessQueue ChangesQueue = new OpenSim.Framework.LocklessQueue(); + + /// + /// A list of actors that should receive collision events. + /// + private readonly List _collisionEventPrim = new List(); + + private readonly HashSet _badCharacter = new HashSet(); + public Dictionary geom_name_map = new Dictionary(); + public Dictionary actor_name_map = new Dictionary(); + + private float contactsurfacelayer = 0.002f; + + private int contactsPerCollision = 80; + internal IntPtr ContactgeomsArray = IntPtr.Zero; + private IntPtr GlobalContactsArray = IntPtr.Zero; + + const int maxContactsbeforedeath = 4000; + private volatile int m_global_contactcount = 0; + + + private readonly IntPtr contactgroup; + + public ContactData[] m_materialContactsData = new ContactData[8]; + + private readonly DoubleDictionary RegionTerrain = new DoubleDictionary(); + private readonly Dictionary TerrainHeightFieldHeights = new Dictionary(); + private readonly Dictionary TerrainHeightFieldHeightsHandlers = new Dictionary(); + + private int m_physicsiterations = 10; + private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag + private readonly PhysicsActor PANull = new NullPhysicsActor(); + private float step_time = 0.0f; + + public IntPtr world; + + private uint obj2LocalID = 0; + private OdeCharacter cc1; + private OdePrim cp1; + private OdeCharacter cc2; + private OdePrim cp2; + + // split the spaces acording to contents type + // ActiveSpace contains characters and active prims + // StaticSpace contains land and other that is mostly static in enviroment + // this can contain subspaces, like the grid in staticspace + // as now space only contains this 2 top spaces + + public IntPtr TopSpace; // the global space + public IntPtr ActiveSpace; // space for active prims + public IntPtr StaticSpace; // space for the static things around + + // some speedup variables + private int spaceGridMaxX; + private int spaceGridMaxY; + private float spacesPerMeter; + + // split static geometry collision into a grid as before + private IntPtr[,] staticPrimspace; + + private Object OdeLock; + private static Object SimulationLock; + + public IMesher mesher; + + private IConfigSource m_config; + + public bool physics_logging = false; + public int physics_logging_interval = 0; + public bool physics_logging_append_existing_logfile = false; + + private Vector3 m_worldOffset = Vector3.Zero; + public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + private PhysicsScene m_parentScene = null; + + private ODERayCastRequestManager m_rayCastManager; + + +/* maybe needed if ode uses tls + private void checkThread() + { + + int th = Thread.CurrentThread.ManagedThreadId; + if(th != threadid) + { + threadid = th; + d.AllocateODEDataForThread(~0U); + } + } + */ + /// + /// Initiailizes the scene + /// Sets many properties that ODE requires to be stable + /// These settings need to be tweaked 'exactly' right or weird stuff happens. + /// + public OdeScene(string sceneIdentifier) + { + m_log + = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); + +// checkThread(); + Name = sceneIdentifier; + + OdeLock = new Object(); + SimulationLock = new Object(); + + nearCallback = near; + + m_rayCastManager = new ODERayCastRequestManager(this); + lock (OdeLock) + { + // Create the world and the first space + try + { + world = d.WorldCreate(); + TopSpace = d.HashSpaceCreate(IntPtr.Zero); + + // now the major subspaces + ActiveSpace = d.HashSpaceCreate(TopSpace); + StaticSpace = d.HashSpaceCreate(TopSpace); + } + catch + { + // i must RtC#FM + } + + d.HashSpaceSetLevels(TopSpace, -2, 8); // cell sizes from .25 to 256 ?? need check what this really does + d.HashSpaceSetLevels(ActiveSpace, -2, 8); + d.HashSpaceSetLevels(StaticSpace, -2, 8); + + // demote to second level + d.SpaceSetSublevel(ActiveSpace, 1); + d.SpaceSetSublevel(StaticSpace, 1); + + contactgroup = d.JointGroupCreate(0); + //contactgroup + + d.WorldSetAutoDisableFlag(world, false); + #if USE_DRAWSTUFF + + Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); + viewthread.Start(); + #endif + } + + _watermap = new float[258 * 258]; + } + +#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 the mesh plugin +// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region ) + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { +// checkThread(); + mesher = meshmerizer; + m_config = config; + +// m_log.WarnFormat("ODE configuration: {0}", d.GetConfiguration("ODE")); + /* + if (region != null) + { + WorldExtents.X = region.RegionSizeX; + WorldExtents.Y = region.RegionSizeY; + } + */ + + // Defaults + + avPIDD = 2200.0f; + avPIDP = 900.0f; + + int contactsPerCollision = 80; + + if (m_config != null) + { + IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; + if (physicsconfig != null) + { + gravityx = physicsconfig.GetFloat("world_gravityx", 0f); + gravityy = physicsconfig.GetFloat("world_gravityy", 0f); + gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f); + + metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f); + + contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); + + ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f); + m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); + + avDensity = physicsconfig.GetFloat("av_density", avDensity); + avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); + avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); + avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f); + + contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); + + geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3); + geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); + geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); + + geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f); + bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20); + + bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f); + bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f); + + forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); + meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); + meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f); + MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f); + m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f); + } + else + { + avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f); + avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f); + } + + physics_logging = physicsconfig.GetBoolean("physics_logging", false); + physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); + physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); + + minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); + maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f); + } + } + + ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); + GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); + + m_materialContactsData[(int)Material.Stone].mu = frictionScale * 0.8f; + m_materialContactsData[(int)Material.Stone].bounce = 0.4f; + + m_materialContactsData[(int)Material.Metal].mu = frictionScale * 0.3f; + m_materialContactsData[(int)Material.Metal].bounce = 0.4f; + + m_materialContactsData[(int)Material.Glass].mu = frictionScale * 0.2f; + m_materialContactsData[(int)Material.Glass].bounce = 0.7f; + + m_materialContactsData[(int)Material.Wood].mu = frictionScale * 0.6f; + m_materialContactsData[(int)Material.Wood].bounce = 0.5f; + + m_materialContactsData[(int)Material.Flesh].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; + + m_materialContactsData[(int)Material.Plastic].mu = frictionScale * 0.4f; + m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; + + m_materialContactsData[(int)Material.Rubber].mu = frictionScale * 0.9f; + m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; + + m_materialContactsData[(int)Material.light].mu = 0.0f; + m_materialContactsData[(int)Material.light].bounce = 0.0f; + + TerrainFriction *= frictionScale; +// AvatarFriction *= frictionScale; + + // Set the gravity,, don't disable things automatically (we set it explicitly on some things) + + d.WorldSetGravity(world, gravityx, gravityy, gravityz); + d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); + + d.WorldSetLinearDamping(world, 0.001f); + d.WorldSetAngularDamping(world, 0.001f); + d.WorldSetAngularDampingThreshold(world, 0f); + d.WorldSetLinearDampingThreshold(world, 0f); + d.WorldSetMaxAngularSpeed(world, 256f); + + d.WorldSetCFM(world,1e-6f); // a bit harder than default + //d.WorldSetCFM(world, 1e-4f); // a bit harder than default + d.WorldSetERP(world, 0.6f); // higher than original + + // Set how many steps we go without running collision testing + // This is in addition to the step size. + // Essentially Steps * m_physicsiterations + d.WorldSetQuickStepNumIterations(world, m_physicsiterations); + d.WorldSetContactMaxCorrectingVel(world, 100.0f); + + spacesPerMeter = 1 / metersInSpace; + spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter); + spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter); + + staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; + + // create all spaces now + int i, j; + IntPtr newspace; + for (i = 0; i < spaceGridMaxX; i++) + for (j = 0; j < spaceGridMaxY; j++) + { + newspace = d.HashSpaceCreate(StaticSpace); + d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); + waitForSpaceUnlock(newspace); + d.SpaceSetSublevel(newspace, 2); + d.HashSpaceSetLevels(newspace, -2, 8); + staticPrimspace[i, j] = newspace; + } + // let this now be real maximum values + spaceGridMaxX--; + spaceGridMaxY--; + } + + internal void waitForSpaceUnlock(IntPtr space) + { + //if (space != IntPtr.Zero) + //while (d.SpaceLockQuery(space)) { } // Wait and do nothing + } + + #region Collision Detection + + // sets a global contact for a joint for contactgeom , and base contact description) + + private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp) + { + if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) + return IntPtr.Zero; + + d.Contact newcontact = new d.Contact(); + newcontact.geom.depth = contactGeom.depth; + newcontact.geom.g1 = contactGeom.g1; + newcontact.geom.g2 = contactGeom.g2; + newcontact.geom.pos = contactGeom.pos; + newcontact.geom.normal = contactGeom.normal; + newcontact.geom.side1 = contactGeom.side1; + newcontact.geom.side2 = contactGeom.side2; + + // this needs bounce also + newcontact.surface.mode = comumContactFlags; + newcontact.surface.mu = mu; + newcontact.surface.bounce = bounce; + newcontact.surface.soft_cfm = comumContactCFM; + if (softerp) + newcontact.surface.soft_erp = comumSoftContactERP; + else + newcontact.surface.soft_erp = comumContactERP; + + IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); + Marshal.StructureToPtr(newcontact, contact, true); + return d.JointCreateContactPtr(world, contactgroup, contact); + } + + + /// + /// This is our near callback. A geometry is near a body + /// + /// The space that contains the geoms. Remember, spaces are also geoms + /// a geometry or space + /// another geometry or space + /// + + private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) + { + if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) + return false; + + IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); + newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); + return true; + } + + + + private void near(IntPtr space, IntPtr g1, IntPtr g2) + { + // no lock here! It's invoked from within Simulate(), which is thread-locked + + if (m_global_contactcount >= maxContactsbeforedeath) + return; + + // Test if we're colliding a geom with a space. + // If so we have to drill down into the space recursively + + if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) + return; + + if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) + { + // We'll be calling near recursivly if one + // of them is a space to find all of the + // contact points in the space + try + { + d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to collide test a space"); + return; + } + //here one should check collisions of geoms inside a space + // but on each space we only should have geoms that not colide amoung each other + // so we don't dig inside spaces + return; + } + + // get geom bodies to check if we already a joint contact + // guess this shouldn't happen now + IntPtr b1 = d.GeomGetBody(g1); + IntPtr b2 = d.GeomGetBody(g2); + + // d.GeomClassID id = d.GeomGetClass(g1); + + // Figure out how many contact points we have + int count = 0; + try + { + // Colliding Geom To Geom + // This portion of the function 'was' blatantly ripped off from BoxStack.cs + + if (g1 == g2) + return; // Can't collide with yourself + + if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) + return; + + count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); + } + catch (SEHException) + { + m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); +// ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + catch (Exception e) + { + m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); + return; + } + + // id contacts done + if (count == 0) + return; + + // try get physical actors + PhysicsActor p1; + PhysicsActor p2; + + if (!actor_name_map.TryGetValue(g1, out p1)) + { + p1 = PANull; + } + + if (!actor_name_map.TryGetValue(g2, out p2)) + { + p2 = PANull; + } + + // update actors collision score + if (p1.CollisionScore >= float.MaxValue - count) + p1.CollisionScore = 0; + p1.CollisionScore += count; + + if (p2.CollisionScore >= float.MaxValue - count) + p2.CollisionScore = 0; + p2.CollisionScore += count; + + + // get first contact + d.ContactGeom curContact = new d.ContactGeom(); + if (!GetCurContactGeom(0, ref curContact)) + return; + // for now it's the one with max depth + ContactPoint maxDepthContact = new ContactPoint( + new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), + new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), + curContact.depth + ); + // do volume detection case + if ( + (p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect) || + (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) + { + collision_accounting_events(p1, p2, maxDepthContact); + return; + } + + // big messy collision analises + float mu = 0; + float bounce = 0; + ContactData contactdata1; + ContactData contactdata2; + bool erpSoft = false; + + String name = null; + bool dop1foot = false; + bool dop2foot = false; + bool ignore = false; + + switch (p1.PhysicsActorType) + { + case (int)ActorTypes.Agent: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + p1.CollidingObj = true; + p2.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + if (p2.Velocity.LengthSquared() > 0.0f) + p2.CollidingObj = true; + dop1foot = true; + break; + default: + ignore=true; // avatar to terrain and water ignored + break; + } + break; + + case (int)ActorTypes.Prim: + switch (p2.PhysicsActorType) + { + case (int)ActorTypes.Agent: + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + dop2foot = true; + if (p1.Velocity.LengthSquared() > 0.0f) + p1.CollidingObj = true; + break; + case (int)ActorTypes.Prim: + if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f) + { + p1.CollidingObj = true; + p2.CollidingObj = true; + } + contactdata1 = p1.ContactData; + contactdata2 = p2.ContactData; + bounce = contactdata1.bounce * contactdata2.bounce; + erpSoft = true; + mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); + + if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) + mu *= frictionMovementMult; + + break; + default: + if (geom_name_map.TryGetValue(g2, out name)) + { + if (name == "Terrain") + { + erpSoft = true; + contactdata1 = p1.ContactData; + bounce = contactdata1.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); + if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + p1.CollidingGround = true; + } + else if (name == "Water") + { + erpSoft = true; + } + } + else + ignore=true; + break; + } + break; + + default: + if (geom_name_map.TryGetValue(g1, out name)) + { + if (name == "Terrain") + { + if (p2.PhysicsActorType == (int)ActorTypes.Prim) + { + erpSoft = true; + p2.CollidingGround = true; + contactdata2 = p2.ContactData; + bounce = contactdata2.bounce * TerrainBounce; + mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); + + if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) + mu *= frictionMovementMult; + } + else + ignore = true; + + } + else if (name == "Water" && + (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) + { + erpSoft = true; + } + } + else + ignore = true; + break; + } + + if (ignore) + return; + + IntPtr Joint; + + int i = 0; + while(true) + { + if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f) + p1.IsColliding = true; + if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) + p2.IsColliding = true; + + Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft); + d.JointAttach(Joint, b1, b2); + + if (++m_global_contactcount >= maxContactsbeforedeath) + break; + + if(++i >= count) + break; + + if (!GetCurContactGeom(i, ref curContact)) + break; + + if (curContact.depth > maxDepthContact.PenetrationDepth) + { + maxDepthContact.Position.X = curContact.pos.X; + maxDepthContact.Position.Y = curContact.pos.Y; + maxDepthContact.Position.Z = curContact.pos.Z; + maxDepthContact.SurfaceNormal.X = curContact.normal.X; + maxDepthContact.SurfaceNormal.Y = curContact.normal.Y; + maxDepthContact.SurfaceNormal.Z = curContact.normal.Z; + maxDepthContact.PenetrationDepth = curContact.depth; + } + } + + collision_accounting_events(p1, p2, maxDepthContact); + +/* + if (notskipedcount > geomContactPointsStartthrottle) + { + // If there are more then 3 contact points, it's likely + // that we've got a pile of objects, so ... + // We don't want to send out hundreds of terse updates over and over again + // so lets throttle them and send them again after it's somewhat sorted out. + this needs checking so out for now + if (b1 != IntPtr.Zero) + p1.ThrottleUpdates = true; + if (b2 != IntPtr.Zero) + p2.ThrottleUpdates = true; + + } + */ + } + + private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) + { + // obj1LocalID = 0; + //returncollisions = false; + obj2LocalID = 0; + //ctype = 0; + //cStartStop = 0; + if (!(p2.SubscribedEvents() || p1.SubscribedEvents())) + return; + + switch ((ActorTypes)p1.PhysicsActorType) + { + case ActorTypes.Agent: + cc1 = (OdeCharacter)p1; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cc1.m_localID, contact); + break; + + case ActorTypes.Prim: + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cc1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cc1.AddCollisionEvent(obj2LocalID, contact); + } + break; + + case ActorTypes.Prim: + + if (p1 is OdePrim) + { + cp1 = (OdePrim)p1; + + // obj1LocalID = cp2.m_localID; + switch ((ActorTypes)p2.PhysicsActorType) + { + case ActorTypes.Agent: + if (p2 is OdeCharacter) + { + cc2 = (OdeCharacter)p2; + obj2LocalID = cc2.m_localID; + if (p2.SubscribedEvents()) + cc2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + case ActorTypes.Prim: + + if (p2 is OdePrim) + { + cp2 = (OdePrim)p2; + obj2LocalID = cp2.m_localID; + if (p2.SubscribedEvents()) + cp2.AddCollisionEvent(cp1.m_localID, contact); + } + break; + + case ActorTypes.Ground: + case ActorTypes.Unknown: + default: + obj2LocalID = 0; + break; + } + if (p1.SubscribedEvents()) + { + contact.SurfaceNormal = -contact.SurfaceNormal; + cp1.AddCollisionEvent(obj2LocalID, contact); + } + } + break; + } + } + + /// + /// This is our collision testing routine in ODE + /// + /// + private void collision_optimized() + { +// _perloopContact.Clear(); +// clear characts IsColliding until we do it some other way + + lock (_characters) + { + foreach (OdeCharacter chr in _characters) + { + // this are odd checks if they are needed something is wrong elsewhere + // keep for now + if (chr == null) + continue; + + if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero) + continue; + + chr.IsColliding = false; + // chr.CollidingGround = false; not done here + chr.CollidingObj = false; + } + } + + // now let ode do its job + // colide active things amoung them + + int st = Util.EnvironmentTickCount(); + int ta; + int ts; + try + { + d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active space collide"); + } + ta = Util.EnvironmentTickCountSubtract(st); + // then active things with static enviroment + try + { + d.SpaceCollide2(ActiveSpace,StaticSpace, IntPtr.Zero, nearCallback); + } + catch (AccessViolationException) + { + m_log.Warn("[PHYSICS]: Unable to Active to static space collide"); + } + ts = Util.EnvironmentTickCountSubtract(st); +// _perloopContact.Clear(); + } + + #endregion + + + public float GetTerrainHeightAtXY(float x, float y) + { + // assumes 1m size grid and constante size square regions + // region offset in mega position + + int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; + + IntPtr heightFieldGeom = IntPtr.Zero; + + // get region map + if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) + return 0f; + + if (heightFieldGeom == IntPtr.Zero) + return 0f; + + if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) + return 0f; + + // TerrainHeightField for ODE as offset 1m + x += 1f - offsetX; + y += 1f - offsetY; + + // make position fit into array + if (x < 0) + x = 0; + if (y < 0) + y = 0; + + // integer indexs + int ix; + int iy; + // interpolators offset + float dx; + float dy; + + int regsize = (int)Constants.RegionSize + 2; // map size see setterrain + + // we still have square fixed size regions + // also flip x and y because of how map is done for ODE fliped axis + // so ix,iy,dx and dy are inter exchanged + if (x < regsize - 1) + { + iy = (int)x; + dy = x - (float)iy; + } + else // out world use external height + { + iy = regsize - 1; + dy = 0; + } + if (y < regsize - 1) + { + ix = (int)y; + dx = y - (float)ix; + } + else + { + ix = regsize - 1; + dx = 0; + } + + float h0; + float h1; + float h2; + + iy *= regsize; + iy += ix; // all indexes have iy + ix + + float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; + + if ((dx + dy) <= 1.0f) + { + h0 = ((float)heights[iy]); // 0,0 vertice + h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 + h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 + } + else + { + h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice + h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 + h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 + } + + return h0 + h1 + h2; + } + + /// + /// Add actor to the list that should receive collision events in the simulate loop. + /// + /// + public void AddCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (!_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Add(obj); + } + } + + /// + /// Remove actor from the list that should receive collision events in the simulate loop. + /// + /// + public void RemoveCollisionEventReporting(PhysicsActor obj) + { + lock (_collisionEventPrim) + { + if (_collisionEventPrim.Contains(obj)) + _collisionEventPrim.Remove(obj); + } + } + + #region Add/Remove Entities + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + Vector3 pos; + pos.X = position.X; + pos.Y = position.Y; + pos.Z = position.Z; + OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun); + newAv.Flying = isFlying; + newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; + + return newAv; + } + + public void AddCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (!_characters.Contains(chr)) + { + _characters.Add(chr); + if (chr.bad) + m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); + } + } + } + + public void RemoveCharacter(OdeCharacter chr) + { + lock (_characters) + { + if (_characters.Contains(chr)) + { + _characters.Remove(chr); + } + } + } + + public void BadCharacter(OdeCharacter chr) + { + lock (_badCharacter) + { + if (!_badCharacter.Contains(chr)) + _badCharacter.Add(chr); + } + } + + public override void RemoveAvatar(PhysicsActor actor) + { + //m_log.Debug("[PHYSICS]:ODELOCK"); + ((OdeCharacter) actor).Destroy(); + } + + private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, + PrimitiveBaseShape pbs, bool isphysical, uint localID) + { + Vector3 pos = position; + Vector3 siz = size; + Quaternion rot = rotation; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); + + lock (_prims) + _prims.Add(newPrim); + } + newPrim.LocalID = localID; + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) + { + // adds active prim.. (ones that should be iterated over in collisions_optimized + lock (_activeprims) + { + if (!_activeprims.Contains(activatePrim)) + _activeprims.Add(activatePrim); + //else + // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent"); + } + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical, uint localid) + { +#if SPAM + m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName); +#endif + + return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid); + } + + public override float TimeDilation + { + get { return m_timeDilation; } + } + + public override bool SupportsNINJAJoints + { + get { return false; } + } + + + public void remActivePrim(OdePrim deactivatePrim) + { + lock (_activeprims) + { + _activeprims.Remove(deactivatePrim); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be + // removed in the next physics simulate pass. + if (prim is OdePrim) + { +// lock (OdeLock) + { + OdePrim p = (OdePrim)prim; + p.setPrimForRemoval(); + } + } + } + /// + /// This is called from within simulate but outside the locked portion + /// We need to do our own locking here + /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in + /// Simulate() -- justincc). + /// + /// Essentially, we need to remove the prim from our space segment, whatever segment it's in. + /// + /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory + /// that the space was using. + /// + /// + public void RemovePrimThreadLocked(OdePrim prim) + { + //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); + lock (prim) + { + RemoveCollisionEventReporting(prim); + lock (_prims) + _prims.Remove(prim); + } + + } + #endregion + + #region Space Separation Calculation + + /// + /// Called when a static prim moves or becomes static + /// Places the prim in a space one the static sub-spaces grid + /// + /// the pointer to the geom that moved + /// the position that the geom moved to + /// a pointer to the space it was in before it was moved. + /// a pointer to the new space it's in + public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) + { + // moves a prim into another static sub-space or from another space into a static sub-space + + // Called ODEPrim so + // it's already in locked space. + + if (geom == IntPtr.Zero) // shouldn't happen + return IntPtr.Zero; + + // get the static sub-space for current position + IntPtr newspace = calculateSpaceForGeom(pos); + + if (newspace == currentspace) // if we are there all done + return newspace; + + // else remove it from its current space + if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + else + { + m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + + " Geom:" + geom); + } + } + else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space + { + currentspace = d.GeomGetSpace(geom); + if (currentspace != IntPtr.Zero) + { + if (d.GeomIsSpace(currentspace)) + { + waitForSpaceUnlock(currentspace); + d.SpaceRemove(currentspace, geom); + } + } + } + + // put the geom in the newspace + waitForSpaceUnlock(newspace); + d.SpaceAdd(newspace, geom); + + // let caller know this newspace + return newspace; + } + + /// + /// Calculates the space the prim should be in by its position + /// + /// + /// a pointer to the space. This could be a new space or reused space. + public IntPtr calculateSpaceForGeom(Vector3 pos) + { + int x, y; + x = (int)(pos.X * spacesPerMeter); + if (x < 0) + x = 0; + else if (x > spaceGridMaxX) + x = spaceGridMaxX; + + y = (int)(pos.Y * spacesPerMeter); + if (y < 0) + y = 0; + else if (y >spaceGridMaxY) + y = spaceGridMaxY; + + IntPtr tmpSpace = staticPrimspace[x, y]; + return tmpSpace; + } + + #endregion + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// + public bool needsMeshing(PrimitiveBaseShape pbs) + { + // most of this is redundant now as the mesher will return null if it cant mesh a prim + // but we still need to check for sculptie meshing being enabled so this is the most + // convenient place to do it for now... + + // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f) + // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString()); + int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry) + { + if(!meshSculptedPrim) + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim + if (!forceSimplePrimMeshing && !pbs.SculptEntry) + { + if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) + || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 + && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) + { + + if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 + && pbs.ProfileHollow == 0 + && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 + && pbs.PathBegin == 0 && pbs.PathEnd == 0 + && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 + && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 + && pbs.PathShearX == 0 && pbs.PathShearY == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } + } + } + + // following code doesn't give meshs to boxes and spheres ever + // and it's odd.. so for now just return true if asked to force meshs + // hopefully mesher will fail if doesn't suport so things still get basic boxes + + if (forceSimplePrimMeshing) + return true; + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + if (pbs.SculptEntry && meshSculptedPrim) + iPropertiesNotSupportedDefault++; + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + public void AddChange(OdePrim prim, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.prim = prim; + item.what = what; + item.arg = arg; + ChangesQueue.Enqueue(item); + } + + /// + /// Called to queue a change to a prim + /// to use in place of old taint mechanism so changes do have a time sequence + /// + public void AddChange(OdeCharacter character, changes what, Object arg) + { + ODEchangeitem item = new ODEchangeitem(); + item.character = character; + 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) + { + if (prim is OdePrim) + { +/* OdePrim taintedprim = ((OdePrim) prim); + lock (_taintedPrimLock) + { + if (!(_taintedPrimH.Contains(taintedprim))) + { + _taintedPrimH.Add(taintedprim); // HashSet for searching + _taintedPrimQ.Enqueue(taintedprim); // List for ordered readout + } + } + */ + return; + } + else if (prim is OdeCharacter) + { + OdeCharacter taintedchar = ((OdeCharacter)prim); + lock (_taintedCharacterLock) + { + if (!(_taintedCharacterH.Contains(taintedchar))) + { + _taintedCharacterH.Add(taintedchar); + _taintedCharacterQ.Enqueue(taintedchar); + if (taintedchar.bad) + m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); + } + } + } + } + + /// + /// This is our main simulate loop + /// It's thread locked by a Mutex in the scene. + /// It holds Collisions, it instructs ODE to step through the physical reactions + /// It moves the objects around in memory + /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) + /// + /// + /// + public override float Simulate(float timeStep) + { + int statstart; + int statchanges = 0; + int statchmove = 0; + int statactmove = 0; + int statray = 0; + int statcol = 0; + int statstep = 0; + int statmovchar = 0; + int statmovprim; + int totjcontact = 0; + + // acumulate time so we can reduce error + step_time += timeStep; + + if (step_time < ODE_STEPSIZE) + return 0; + + if (framecount >= int.MaxValue) + framecount = 0; + + framecount++; + + int curphysiteractions = m_physicsiterations; + + if (step_time >= m_SkipFramesAtms) + { + // if in trouble reduce step resolution + curphysiteractions /= 2; + } + + int nodeframes = 0; + +// checkThread(); + + lock (SimulationLock) + { + // adjust number of iterations per step + try + { + d.WorldSetQuickStepNumIterations(world, curphysiteractions); + } + catch (StackOverflowException) + { + m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim."); +// ode.drelease(world); + base.TriggerPhysicsBasedRestart(); + } + + + while (step_time >= ODE_STEPSIZE && nodeframes < 10) //limit number of steps so we don't say here for ever + { + try + { + statstart = Util.EnvironmentTickCount(); + + // clear pointer/counter to contacts to pass into joints + m_global_contactcount = 0; + + // do characters requested changes + + OdeCharacter character; + int numtaints; + lock (_taintedCharacterLock) + { + numtaints = _taintedCharacterQ.Count; + // if (numtaints > 50) + // numtaints = 50; + while (numtaints > 0) + { + character = _taintedCharacterQ.Dequeue(); + character.ProcessTaints(ODE_STEPSIZE); + _taintedCharacterH.Remove(character); + numtaints--; + } + } + // do other objects requested changes + + ODEchangeitem item; + + if(ChangesQueue.Count >0) + { + int ttmpstart = Util.EnvironmentTickCount(); + int ttmp; + int ttmp2; + + while(ChangesQueue.Dequeue(out item)) + { + if (item.prim != null) + { + try + { + if (item.prim.DoAChange(item.what, item.arg)) + RemovePrimThreadLocked(item.prim); + } + catch { }; + } + ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp > 20) + break; + } + + ttmp2 = Util.EnvironmentTickCountSubtract(ttmpstart); + if (ttmp2 > 50) + ttmp2 = 0; + + } + + statchanges += Util.EnvironmentTickCountSubtract(statstart); + + // Move characters + lock (_characters) + { + List defects = new List(); + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + actor.Move(ODE_STEPSIZE, defects); + } + if (defects.Count != 0) + { + foreach (OdeCharacter defect in defects) + { + RemoveCharacter(defect); + } + } + } + statchmove += Util.EnvironmentTickCountSubtract(statstart); + + // Move other active objects + lock (_activeprims) + { + foreach (OdePrim aprim in _activeprims) + { + aprim.CollisionScore = 0; + aprim.IsColliding = false; + aprim.Move(); + } + } + + statactmove += Util.EnvironmentTickCountSubtract(statstart); + //if ((framecount % m_randomizeWater) == 0) + // randomizeWater(waterlevel); + + m_rayCastManager.ProcessQueuedRequests(); + + statray += Util.EnvironmentTickCountSubtract(statstart); + collision_optimized(); + statcol += Util.EnvironmentTickCountSubtract(statstart); + + lock (_collisionEventPrim) + { + foreach (PhysicsActor obj in _collisionEventPrim) + { + if (obj == null) + continue; + + switch ((ActorTypes)obj.PhysicsActorType) + { + case ActorTypes.Agent: + OdeCharacter cobj = (OdeCharacter)obj; + cobj.AddCollisionFrameTime((int)(ODE_STEPSIZE*1000.0f)); + cobj.SendCollisions(); + break; + + case ActorTypes.Prim: + OdePrim pobj = (OdePrim)obj; + pobj.SendCollisions(); + break; + } + } + } + + d.WorldQuickStep(world, ODE_STEPSIZE); + statstep += Util.EnvironmentTickCountSubtract(statstart); + d.JointGroupEmpty(contactgroup); + totjcontact += m_global_contactcount; + //ode.dunlock(world); + } + catch (Exception e) + { + m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); +// ode.dunlock(world); + } + + step_time -= ODE_STEPSIZE; + nodeframes++; + } + + statstart = Util.EnvironmentTickCount(); + + lock (_characters) + { + foreach (OdeCharacter actor in _characters) + { + if (actor != null) + { + if (actor.bad) + m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); + + actor.UpdatePositionAndVelocity(); + } + } + } + + lock (_badCharacter) + { + if (_badCharacter.Count > 0) + { + foreach (OdeCharacter chr in _badCharacter) + { + RemoveCharacter(chr); + } + + _badCharacter.Clear(); + } + } + statmovchar = Util.EnvironmentTickCountSubtract(statstart); + + lock (_activeprims) + { + { + foreach (OdePrim actor in _activeprims) + { + if (actor.IsPhysical) + { + actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE); + } + } + } + } + + statmovprim = Util.EnvironmentTickCountSubtract(statstart); + + int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); + int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); + int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); + int nbodies = d.NTotalBodies; + + // Finished with all sim stepping. If requested, dump world state to file for debugging. + // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? + // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? + if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) + { + string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename + string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file + + if (physics_logging_append_existing_logfile) + { + string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; + TextWriter fwriter = File.AppendText(fname); + fwriter.WriteLine(header); + fwriter.Close(); + } + + d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); + } + + // think time dilation is not a physics issue alone.. but ok let's fake something + if (step_time < ODE_STEPSIZE) // we did the required loops + m_timeDilation = 1.0f; + else + { // we didn't forget the lost ones and let user know something + m_timeDilation = 1 - step_time / timeStep; + if (m_timeDilation < 0) + m_timeDilation = 0; + step_time = 0; + } + } + +// return nodeframes * ODE_STEPSIZE; // return real simulated time + return 1000 * nodeframes; // return steps for now * 1000 to keep core happy + } + + /// + public override void GetResults() + { + } + + public override bool IsThreaded + { + // for now we won't be multithreaded + get { return (false); } + } + + #region ODE Specific Terrain Fixes + public float[] ResizeTerrain512NearestNeighbour(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x]; + } + } + + // Resize using Nearest Neighbour + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512, 512]; + for (int y = 0; y < WorldExtents.Y; y++) + { + for (int x = 0; x < WorldExtents.X; x++) + { + resultarr2[y * 2, x * 2] = resultarr[y, x]; + + if (y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x]; + } + if (x < WorldExtents.X) + { + resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x]; + } + if (x < WorldExtents.X && y < WorldExtents.Y) + { + resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x]; + } + } + } + + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (resultarr2[y, x] <= 0) + returnarr[i] = 0.0000001f; + else + returnarr[i] = resultarr2[y, x]; + + i++; + } + } + + return returnarr; + } + + public float[] ResizeTerrain512Interpolation(float[] heightMap) + { + float[] returnarr = new float[262144]; + float[,] resultarr = new float[512,512]; + + // Filling out the array into its multi-dimensional components + for (int y = 0; y < 256; y++) + { + for (int x = 0; x < 256; x++) + { + resultarr[y, x] = heightMap[y * 256 + x]; + } + } + + // Resize using interpolation + + // This particular way is quick but it only works on a multiple of the original + + // The idea behind this method can be described with the following diagrams + // second pass and third pass happen in the same loop really.. just separated + // them to show what this does. + + // First Pass + // ResultArr: + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + // 1,1,1,1,1,1 + + // Second Pass + // ResultArr2: + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + // ,,,,,,,,,, + // 1,,1,,1,,1,,1,,1, + + // Third pass fills in the blanks + // ResultArr2: + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + // 1,1,1,1,1,1,1,1,1,1,1,1 + + // X,Y = . + // X+1,y = ^ + // X,Y+1 = * + // X+1,Y+1 = # + + // Filling in like this; + // .* + // ^# + // 1st . + // 2nd * + // 3rd ^ + // 4th # + // on single loop. + + float[,] resultarr2 = new float[512,512]; + for (int y = 0; y < (int)Constants.RegionSize; y++) + { + for (int x = 0; x < (int)Constants.RegionSize; x++) + { + resultarr2[y*2, x*2] = resultarr[y, x]; + + if (y < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2); + } + } + else + { + resultarr2[(y*2) + 1, x*2] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize) + { + if (x + 1 < (int)Constants.RegionSize) + { + if (y + 1 < (int)Constants.RegionSize) + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2); + } + } + else + { + resultarr2[y*2, (x*2) + 1] = resultarr[y, x]; + } + } + if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize) + { + if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize)) + { + resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + + resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); + } + else + { + resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; + } + } + } + } + //Flatten out the array + int i = 0; + for (int y = 0; y < 512; y++) + { + for (int x = 0; x < 512; x++) + { + if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) + { + m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0"); + resultarr2[y, x] = 0; + } + returnarr[i] = resultarr2[y, x]; + i++; + } + } + + return returnarr; + } + + #endregion + + public override void SetTerrain(float[] heightMap) + { + if (m_worldOffset != Vector3.Zero && m_parentScene != null) + { + if (m_parentScene is OdeScene) + { + ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); + } + } + else + { + SetTerrain(heightMap, m_worldOffset); + } + } + + public override void CombineTerrain(float[] heightMap, Vector3 pOffset) + { + SetTerrain(heightMap, pOffset); + } + + public void SetTerrain(float[] heightMap, Vector3 pOffset) + { + + float[] _heightmap; + _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; + + uint heightmapWidth = Constants.RegionSize + 2; + uint heightmapHeight = Constants.RegionSize + 2; + + uint heightmapWidthSamples; + + uint heightmapHeightSamples; + + heightmapWidthSamples = (uint)Constants.RegionSize + 2; + heightmapHeightSamples = (uint)Constants.RegionSize + 2; + + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 10f; + const int wrap = 0; + + int regionsize = (int) Constants.RegionSize + 2; + + float hfmin = float.MaxValue; + float hfmax = float.MinValue; + float val; + int xx; + int yy; + + int maxXXYY = regionsize - 3; + // flipping map adding one margin all around so things don't fall in edges + + int xt = 0; + xx = 0; + + for (int x = 0; x < heightmapWidthSamples; x++) + { + if (x > 1 && xx < maxXXYY) + xx++; + yy = 0; + for (int y = 0; y < heightmapHeightSamples; y++) + { + if (y > 1 && y < maxXXYY) + yy += (int)Constants.RegionSize; + + val = heightMap[yy + xx]; + _heightmap[xt + y] = val; + + if (hfmin > val) + hfmin = val; + if (hfmax < val) + hfmax = val; + + } + + xt += regionsize; + } + lock (OdeLock) + { + IntPtr GroundGeom = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) + { + RegionTerrain.Remove(pOffset); + if (GroundGeom != IntPtr.Zero) + { + if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) + { + TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); + TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); + TerrainHeightFieldHeights.Remove(GroundGeom); + } + d.SpaceRemove(StaticSpace, GroundGeom); + d.GeomDestroy(GroundGeom); + } + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + + GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); + + d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + + d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); + GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (GroundGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); + d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[GroundGeom] = "Terrain"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + + q1 = q1 * q2; + + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(GroundGeom, ref R); + d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f - 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f - 0.5f, 0); + IntPtr testGround = IntPtr.Zero; + if (RegionTerrain.TryGetValue(pOffset, out testGround)) + { + RegionTerrain.Remove(pOffset); + } + RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); +// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap); + TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); + TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); + + } + } + + public override void DeleteTerrain() + { + } + + public float GetWaterLevel() + { + return waterlevel; + } + + public override bool SupportsCombining() + { + return true; + } +/* + public override void UnCombine(PhysicsScene pScene) + { + IntPtr localGround = IntPtr.Zero; +// float[] localHeightfield; + bool proceed = false; + List geomDestroyList = new List(); + + lock (OdeLock) + { + if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) + { + foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) + { + if (geom == localGround) + { +// localHeightfield = TerrainHeightFieldHeights[geom]; + proceed = true; + } + else + { + geomDestroyList.Add(geom); + } + } + + if (proceed) + { + m_worldOffset = Vector3.Zero; + WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); + m_parentScene = null; + + foreach (IntPtr g in geomDestroyList) + { + // removingHeightField needs to be done or the garbage collector will + // collect the terrain data before we tell ODE to destroy it causing + // memory corruption + if (TerrainHeightFieldHeights.ContainsKey(g)) + { +// float[] removingHeightField = TerrainHeightFieldHeights[g]; + TerrainHeightFieldHeights.Remove(g); + + if (RegionTerrain.ContainsKey(g)) + { + RegionTerrain.Remove(g); + } + + d.GeomDestroy(g); + //removingHeightField = new float[0]; + } + } + + } + else + { + m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); + } + } + } + } +*/ + public override void SetWaterLevel(float baseheight) + { + waterlevel = baseheight; + randomizeWater(waterlevel); + } + + public void randomizeWater(float baseheight) + { + const uint heightmapWidth = m_regionWidth + 2; + const uint heightmapHeight = m_regionHeight + 2; + const uint heightmapWidthSamples = m_regionWidth + 2; + const uint heightmapHeightSamples = m_regionHeight + 2; + const float scale = 1.0f; + const float offset = 0.0f; + const float thickness = 2.9f; + const int wrap = 0; + + for (int i = 0; i < (258 * 258); i++) + { + _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); + // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); + } + + lock (OdeLock) + { + if (WaterGeom != IntPtr.Zero) + { + d.SpaceRemove(StaticSpace, WaterGeom); + } + IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); + d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, + (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, + offset, thickness, wrap); + d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); + WaterGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1); + if (WaterGeom != IntPtr.Zero) + { + d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); + d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); + + } + geom_name_map[WaterGeom] = "Water"; + + d.Matrix3 R = new d.Matrix3(); + + Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); + Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); + + q1 = q1 * q2; + Vector3 v3; + float angle; + q1.GetAxisAngle(out v3, out angle); + + d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); + d.GeomSetRotation(WaterGeom, ref R); + d.GeomSetPosition(WaterGeom, 128, 128, 0); + + } + + } + + public override void Dispose() + { + m_rayCastManager.Dispose(); + m_rayCastManager = null; + + lock (OdeLock) + { + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + RemovePrim(prm); + } + } + + if (ContactgeomsArray != IntPtr.Zero) + Marshal.FreeHGlobal(ContactgeomsArray); + if (GlobalContactsArray != IntPtr.Zero) + Marshal.FreeHGlobal(GlobalContactsArray); + + d.WorldDestroy(world); + //d.CloseODE(); + } + } + + public override Dictionary GetTopColliders() + { + Dictionary returncolliders = new Dictionary(); + int cnt = 0; + lock (_prims) + { + foreach (OdePrim prm in _prims) + { + if (prm.CollisionScore > 0) + { + returncolliders.Add(prm.m_localID, prm.CollisionScore); + cnt++; + prm.CollisionScore = 0f; + if (cnt > 25) + { + break; + } + } + } + } + return returncolliders; + } + + public override bool SupportsRayCast() + { + return true; + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, retMethod); + } + } + + public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null) + { + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) + { + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) + { + if (retMethod != null && actor !=null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod); + } + } + + public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) + { + if (retMethod != null && actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return; + if (geom == IntPtr.Zero) + return; + + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + } + } + + // don't like this + public override List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + if (actor != null) + { + IntPtr geom; + if (actor is OdePrim) + geom = ((OdePrim)actor).prim_geom; + else if (actor is OdeCharacter) + geom = ((OdePrim)actor).prim_geom; + else + return new List(); + if (geom == IntPtr.Zero) + return new List(); + + ContactResult[] ourResults = null; + RayCallback retMethod = delegate(List results) + { + ourResults = new ContactResult[results.Count]; + results.CopyTo(ourResults, 0); + }; + int waitTime = 0; + m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod); + while (ourResults == null && waitTime < 1000) + { + Thread.Sleep(1); + waitTime++; + } + if (ourResults == null) + return new List(); + return new List(ourResults); + } + return new List(); + } + +#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); + lock (_characters) + { + 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 + } +} diff --git a/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs b/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs new file mode 100644 index 0000000..aefad3a --- /dev/null +++ b/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs @@ -0,0 +1,99 @@ +/* + * 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 OdeAPI; + +namespace ODEDrawstuff +{ +/*#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); + } +} -- cgit v1.1