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 From c75fa8b8a1c4ba9235074e84f9a363483c5220f7 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 15:28:13 +0000 Subject: changes in physics manager, needed for UbitODE --- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 19 +++++++++++++++++++ OpenSim/Region/Physics/Manager/PhysicsScene.cs | 10 +++++++++- 2 files changed, 28 insertions(+), 1 deletion(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index 0587054..e1a68be 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -68,6 +68,17 @@ namespace OpenSim.Region.Physics.Manager } } + public struct ContactData + { + public float mu; + public float bounce; + + public ContactData(float _mu, float _bounce) + { + mu = _mu; + bounce = _bounce; + } + } /// /// Used to pass collision information to OnCollisionUpdate listeners. /// @@ -143,6 +154,14 @@ namespace OpenSim.Region.Physics.Manager get { return new NullPhysicsActor(); } } + + public virtual bool Building { get; set; } + + public virtual ContactData ContactData + { + get { return new ContactData(0, 0); } + } + public abstract bool Stopped { get; } public abstract Vector3 Size { get; set; } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 514d9ad..3db71e5 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -225,7 +225,7 @@ namespace OpenSim.Region.Physics.Manager } public virtual void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents) {} - + public virtual void CombineTerrain(float[] heightMap, Vector3 pOffset) {} public virtual void UnCombine(PhysicsScene pScene) {} /// @@ -263,5 +263,13 @@ namespace OpenSim.Region.Physics.Manager { return new List(); } + + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod){} + public virtual void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) { } + public virtual List RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count) + { + return new List(); + } + } } -- cgit v1.1 From 6cc9aa30ac40ecb62870d8ecf566680a1a83d630 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 15:45:44 +0000 Subject: first change in SOP. in AddPrimShape(..) give physics the world rotation and not local offset. Currently physics interface only knows about world frame --- OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index ac39b6b..1696dc5 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -1539,7 +1539,8 @@ namespace OpenSim.Region.Framework.Scenes Shape, AbsolutePosition, Scale, - RotationOffset, +// RotationOffset, + GetWorldRotation(), // physics wants world rotation RigidBody, m_localId); } @@ -4366,7 +4367,8 @@ namespace OpenSim.Region.Framework.Scenes Shape, AbsolutePosition, Scale, - RotationOffset, +// RotationOffset, + GetWorldRotation(), //physics wants world rotation like all other functions send UsePhysics, m_localId); -- cgit v1.1 From 6af01f6767838235091b9e34cdaea05951d68f68 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 8 Feb 2012 23:14:53 +0000 Subject: initial introdution of physics actor building control. --- .../Avatar/Attachments/AttachmentsModule.cs | 2 +- OpenSim/Region/Framework/Scenes/Scene.cs | 2 +- .../Region/Framework/Scenes/SceneObjectGroup.cs | 47 +++++++++++++++++++--- OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 18 ++++++++- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 2 +- 5 files changed, 60 insertions(+), 11 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs b/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs index 5ca2ce4..c5cec59 100644 --- a/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs +++ b/OpenSim/Region/CoreModules/Avatar/Attachments/AttachmentsModule.cs @@ -417,7 +417,7 @@ namespace OpenSim.Region.CoreModules.Avatar.Attachments so.AttachedAvatar = UUID.Zero; rootPart.SetParentLocalId(0); so.ClearPartAttachmentData(); - rootPart.ApplyPhysics(rootPart.GetEffectiveObjectFlags(), rootPart.VolumeDetectActive); + rootPart.ApplyPhysics(rootPart.GetEffectiveObjectFlags(), rootPart.VolumeDetectActive,false); so.HasGroupChanged = true; rootPart.Rezzed = DateTime.Now; rootPart.RemFlag(PrimFlags.TemporaryOnRez); diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs index ec5cf32..456e86b 100644 --- a/OpenSim/Region/Framework/Scenes/Scene.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.cs @@ -4766,7 +4766,7 @@ Environment.Exit(1); bool wasUsingPhysics = ((jointProxyObject.Flags & PrimFlags.Physics) != 0); if (wasUsingPhysics) { - jointProxyObject.UpdatePrimFlags(false, false, true, false); // FIXME: possible deadlock here; check to make sure all the scene alterations set into motion here won't deadlock + jointProxyObject.UpdatePrimFlags(false, false, true, false,false); // FIXME: possible deadlock here; check to make sure all the scene alterations set into motion here won't deadlock } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 46c4d7b..2a3d735 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -1281,7 +1281,8 @@ namespace OpenSim.Region.Framework.Scenes m_rootPart.SetParentLocalId(0); AttachmentPoint = (byte)0; - m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); + // must check if buildind should be true or false here + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive,false); HasGroupChanged = true; RootPart.Rezzed = DateTime.Now; RootPart.RemFlag(PrimFlags.TemporaryOnRez); @@ -1581,21 +1582,33 @@ namespace OpenSim.Region.Framework.Scenes public void ApplyPhysics() { // Apply physics to the root prim - m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); + // m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); // Apply physics to child prims SceneObjectPart[] parts = m_parts.GetArray(); if (parts.Length > 1) { + ResetChildPrimPhysicsPositions(); + + // Apply physics to the root prim + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, true); for (int i = 0; i < parts.Length; i++) { SceneObjectPart part = parts[i]; if (part.LocalId != m_rootPart.LocalId) - part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive); - } +// part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive); + part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive, true); + } // Hack to get the physics scene geometries in the right spot - ResetChildPrimPhysicsPositions(); +// ResetChildPrimPhysicsPositions(); + if (m_rootPart.PhysActor != null) + m_rootPart.PhysActor.Building = false; + } + else + { + // Apply physics to the root prim + m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, false); } } @@ -1829,13 +1842,16 @@ namespace OpenSim.Region.Framework.Scenes pbs, newPart.AbsolutePosition, newPart.Scale, - newPart.RotationOffset, + //newPart.RotationOffset, + newPart.GetWorldRotation(), part.PhysActor.IsPhysical, newPart.LocalId); newPart.DoPhysicsPropertyUpdate(part.PhysActor.IsPhysical, true); } } + if (dupe.m_rootPart.PhysActor != null && userExposed) + dupe.m_rootPart.PhysActor.Building = false; // tell physics to finish building if (userExposed) { @@ -2849,12 +2865,31 @@ namespace OpenSim.Region.Framework.Scenes } } +/* RootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); for (int i = 0; i < parts.Length; i++) { if (parts[i] != RootPart) parts[i].UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect); } +*/ + if (parts.Length > 1) + { + m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, true); + + for (int i = 0; i < parts.Length; i++) + { + + if (parts[i].UUID != m_rootPart.UUID) + parts[i].UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, true); + } + + if (m_rootPart.PhysActor != null) + m_rootPart.PhysActor.Building = false; + } + else + m_rootPart.UpdatePrimFlags(UsePhysics, SetTemporary, SetPhantom, SetVolumeDetect, false); + } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 1696dc5..577c0d3 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -1503,7 +1503,8 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive) +// public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive) + public void ApplyPhysics(uint rootObjectFlags, bool VolumeDetectActive, bool building) { if (!ParentGroup.Scene.CollidablePrims) return; @@ -1557,6 +1558,8 @@ namespace OpenSim.Region.Framework.Scenes PhysActor.SetMaterial(Material); DoPhysicsPropertyUpdate(RigidBody, true); PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0); + if (!building) + PhysActor.Building = false; } } } @@ -1792,6 +1795,10 @@ namespace OpenSim.Region.Framework.Scenes if (!isNew) ParentGroup.Scene.RemovePhysicalPrim(1); + Velocity = new Vector3(0, 0, 0); + Acceleration = new Vector3(0, 0, 0); + AngularVelocity = new Vector3(0, 0, 0); + PhysActor.OnRequestTerseUpdate -= PhysicsRequestingTerseUpdate; PhysActor.OnOutOfBounds -= PhysicsOutOfBounds; PhysActor.delink(); @@ -4268,7 +4275,8 @@ namespace OpenSim.Region.Framework.Scenes /// /// /// - public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD) +// public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD) + public void UpdatePrimFlags(bool UsePhysics, bool SetTemporary, bool SetPhantom, bool SetVD, bool building) { bool wasUsingPhysics = ((Flags & PrimFlags.Physics) != 0); bool wasTemporary = ((Flags & PrimFlags.TemporaryOnRez) != 0); @@ -4286,6 +4294,9 @@ namespace OpenSim.Region.Framework.Scenes // that... // ... if VD is changed, all others are not. // ... if one of the others is changed, VD is not. + // do this first + if (building && PhysActor != null && PhysActor.Building != building) + PhysActor.Building = building; if (SetVD) // VD is active, special logic applies { // State machine logic for VolumeDetect @@ -4448,6 +4459,9 @@ namespace OpenSim.Region.Framework.Scenes } // m_log.Debug("Update: PHY:" + UsePhysics.ToString() + ", T:" + IsTemporary.ToString() + ", PHA:" + IsPhantom.ToString() + " S:" + CastsShadows.ToString()); + // and last in case we have a new actor and not building + if (PhysActor != null && PhysActor.Building != building) + PhysActor.Building = building; if (ParentGroup != null) { ParentGroup.HasGroupChanged = true; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 1a53c99..490c178 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2062,7 +2062,7 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } - m_building = false; // REMOVE THIS LATER +// m_building = false; // REMOVE THIS LATER if (m_isphysical && Body == IntPtr.Zero) -- cgit v1.1 From 7cf73cb92af7d00f9d7a98c1762162d597418a3b Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 10 Feb 2012 22:43:51 +0000 Subject: Changes to vehicles code etc. Includes some debug aids to remove later. --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 296 +++++---------------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +- OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 85 +++++- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 4 files changed, 145 insertions(+), 243 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 363cbef..6b323fb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -629,36 +629,31 @@ namespace OpenSim.Region.Physics.OdePlugin 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; + d.Vector3 dtorque = new d.Vector3(); // 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 + 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_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; - } + 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); } - m_lmEfect *= (1 - 1.0f / m_linearMotorDecayTimescale); + else + { + force.X += tmpV.X; + force.Y += tmpV.Y; + force.Z += tmpV.Z; + } + m_lmEfect *= (1.0f - 1.0f / m_linearMotorDecayTimescale); } else m_lmEfect = 0; @@ -719,30 +714,35 @@ namespace OpenSim.Region.Physics.OdePlugin 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; + Vector3 atAxis; + atAxis = Xrot(rotq); // where are we pointing to + atAxis *= len; // make it same size as world velocity vector + tmpV = -atAxis; // oposite direction + atAxis -= curVel; // error to one direction + len = atAxis.LengthSquared(); + tmpV -= curVel; // error to oposite + float lens = tmpV.LengthSquared(); + if (len > 0.01 || lens > 0.01) // do nothing if close enougth + { + if (len < lens) + tmpV = atAxis; + + tmpV *= (m_linearDeflectionEfficiency / m_linearDeflectionTimescale); // error to correct in this timestep + force.X += tmpV.X; + force.Y += tmpV.Y; + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) == 0) + force.Z += tmpV.Z; + } } // angular motor if (m_amEfect > 0.01 && m_angularMotorTimescale < 1000) { tmpV = m_angularMotorDirection - curLocalAngVel; // velocity error - 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; - } + tmpV *= m_amEfect / m_angularMotorTimescale; // error to correct in this timestep + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; m_amEfect *= (1 - 1.0f / m_angularMotorDecayTimescale); } else @@ -751,85 +751,64 @@ namespace OpenSim.Region.Physics.OdePlugin // 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; + torque.X -= curLocalAngVel.X / m_angularFrictionTimescale.X; + torque.Y -= curLocalAngVel.Y / m_angularFrictionTimescale.Y; + torque.Z -= curLocalAngVel.Z / m_angularFrictionTimescale.Z; } // angular deflection if (m_angularDeflectionEfficiency > 0) { - doathing = false; - float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale / m_angularDeflectionTimescale /_pParentScene.ODE_STEPSIZE; - tmpV.X = 0; - if (Math.Abs(curLocalVel.Z) > 0.01) + Vector3 dirv; + + if (curLocalVel.X > 0.01f) + dirv = curLocalVel; + else if (curLocalVel.X < -0.01f) + // use oposite + dirv = -curLocalVel; + else { - tmpV.Y = -(float)Math.Atan2(curLocalVel.Z, curLocalVel.X) * ftmp; - doathing = true; + // make it fall into small positive x case + dirv.X = 0.01f; + dirv.Y = curLocalVel.Y; + dirv.Z = curLocalVel.Z; } - else - tmpV.Y = 0; - if (Math.Abs(curLocalVel.Y) > 0.01) + + float ftmp = m_angularDeflectionEfficiency / m_angularDeflectionTimescale; + + if (Math.Abs(dirv.Z) > 0.01) { - tmpV.Z = (float)Math.Atan2(curLocalVel.Y, curLocalVel.X) * ftmp; - doathing = true; + torque.Y += - (float)Math.Atan2(dirv.Z, dirv.X) * ftmp; } - else - tmpV.Z = 0; - if (doathing) + if (Math.Abs(dirv.Y) > 0.01) { - tmpV *= m_referenceFrame; // to object - dtorque.X += tmpV.X; - dtorque.Y += tmpV.Y; - dtorque.Z += tmpV.Z; + torque.Z += (float)Math.Atan2(dirv.Y, dirv.X) * ftmp; } } // 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; + torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2; } 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; + torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2; } - tmpV.Z = 0; - - if (m_bankingEfficiency == 0 || Math.Abs(roll) < 0.01) - tmpV.Z = 0; - else + if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01) { float broll = -roll * m_bankingEfficiency; ; if (m_bankingMix != 0) @@ -839,146 +818,10 @@ namespace OpenSim.Region.Physics.OdePlugin 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; + torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale; } } - /* - 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); @@ -988,8 +831,13 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetForce(Body, force.X, force.Y, force.Z); } - if (dtorque.X != 0 || dtorque.Y != 0 || dtorque.Z != 0) + if (torque.X != 0 || torque.Y != 0 || torque.Z != 0) { + torque *= m_referenceFrame; // to object frame + dtorque.X = torque.X; + dtorque.Y = torque.Y; + dtorque.Z = torque.Z; + d.MultiplyM3V3(out dvtmp, ref dmass.I, ref dtorque); d.BodyAddRelTorque(Body, dvtmp.X, dvtmp.Y, dvtmp.Z); // add torque in object frame } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 490c178..7718a29 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1198,7 +1198,7 @@ namespace OpenSim.Region.Physics.OdePlugin } Body = IntPtr.Zero; hasOOBoffsetFromMesh = false; -// CalcPrimBodyData(); + CalcPrimBodyData(); } private void ChildSetGeom(OdePrim odePrim) @@ -1223,7 +1223,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - /* if (m_targetSpace != _parent_scene.ActiveSpace) { m_targetSpace = _parent_scene.ActiveSpace; @@ -1238,7 +1237,6 @@ namespace OpenSim.Region.Physics.OdePlugin } d.SpaceAdd(m_targetSpace, prim_geom); } - */ d.GeomEnable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomEnable(prm.prim_geom); @@ -1256,7 +1254,6 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical && Body != IntPtr.Zero && prim_geom != IntPtr.Zero) { - /* if (m_targetSpace == _parent_scene.ActiveSpace) { foreach (OdePrim prm in childrenPrim) @@ -1270,7 +1267,6 @@ namespace OpenSim.Region.Physics.OdePlugin d.SpaceRemove(m_targetSpace, prim_geom); m_targetSpace = IntPtr.Zero; } - */ d.GeomDisable(prim_geom); foreach (OdePrim prm in childrenPrim) d.GeomDisable(prm.prim_geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index c0c7ff3..2b6bc59 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -59,6 +59,7 @@ namespace OdeAPI { public static dReal Infinity = dReal.MaxValue; public static int NTotalBodies = 0; + public static int NTotalGeoms = 0; #region Flags and Enumerations @@ -420,7 +421,7 @@ namespace OdeAPI [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dBodyCreate"), SuppressUnmanagedCodeSecurity] public static extern IntPtr BodyiCreate(IntPtr world); - public static IntPtr BodyCreate(IntPtr world) + public static IntPtr BodyCreate(IntPtr world) { NTotalBodies++; return BodyiCreate(world); @@ -689,22 +690,52 @@ namespace OdeAPI 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); + public static extern IntPtr CreateiBox(IntPtr space, dReal lx, dReal ly, dReal lz); + public static IntPtr CreateBox(IntPtr space, dReal lx, dReal ly, dReal lz) + { + NTotalGeoms++; + return CreateiBox(space, lx, ly, lz); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCapsule"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length); + public static extern IntPtr CreateiCapsule(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCapsule(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCapsule(space, radius, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateConvex"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static extern IntPtr CreateiConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons); + public static IntPtr CreateConvex(IntPtr space, dReal[] planes, int planeCount, dReal[] points, int pointCount, int[] polygons) + { + NTotalGeoms++; + return CreateiConvex(space, planes, planeCount, points, pointCount, polygons); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateCylinder"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length); + public static extern IntPtr CreateiCylinder(IntPtr space, dReal radius, dReal length); + public static IntPtr CreateCylinder(IntPtr space, dReal radius, dReal length) + { + NTotalGeoms++; + return CreateiCylinder(space, radius, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateHeightfield"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static extern IntPtr CreateiHeightfield(IntPtr space, IntPtr data, int bPlaceable); + public static IntPtr CreateHeightfield(IntPtr space, IntPtr data, int bPlaceable) + { + NTotalGeoms++; + return CreateiHeightfield(space, data, bPlaceable); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeom"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateGeom(int classnum); + public static extern IntPtr CreateiGeom(int classnum); + public static IntPtr CreateGeom(int classnum) + { + NTotalGeoms++; + return CreateiGeom(classnum); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateGeomClass"), SuppressUnmanagedCodeSecurity] public static extern int CreateGeomClass(ref GeomClass classptr); @@ -713,19 +744,39 @@ namespace OdeAPI 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); + public static extern IntPtr CreateiPlane(IntPtr space, dReal a, dReal b, dReal c, dReal d); + public static IntPtr CreatePlane(IntPtr space, dReal a, dReal b, dReal c, dReal d) + { + NTotalGeoms++; + return CreateiPlane(space, a, b, c, d); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateRay"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateRay(IntPtr space, dReal length); + public static extern IntPtr CreateiRay(IntPtr space, dReal length); + public static IntPtr CreateRay(IntPtr space, dReal length) + { + NTotalGeoms++; + return CreateiRay(space, length); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateSphere"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateSphere(IntPtr space, dReal radius); + public static extern IntPtr CreateiSphere(IntPtr space, dReal radius); + public static IntPtr CreateSphere(IntPtr space, dReal radius) + { + NTotalGeoms++; + return CreateiSphere(space, radius); + } [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dCreateTriMesh"), SuppressUnmanagedCodeSecurity] - public static extern IntPtr CreateTriMesh(IntPtr space, IntPtr data, + public static extern IntPtr CreateiTriMesh(IntPtr space, IntPtr data, TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback); - - [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] + public static IntPtr CreateTriMesh(IntPtr space, IntPtr data, + TriCallback callback, TriArrayCallback arrayCallback, TriRayCallback rayCallback) + { + NTotalGeoms++; + return CreateiTriMesh(space, data, callback, arrayCallback, rayCallback); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDot"), SuppressUnmanagedCodeSecurity] public static extern dReal Dot(ref dReal X0, ref dReal X1, int n); [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dDQfromW"), SuppressUnmanagedCodeSecurity] @@ -798,7 +849,13 @@ namespace OdeAPI 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); + public static extern void GeomiDestroy(IntPtr geom); + public static void GeomDestroy(IntPtr geom) + { + NTotalGeoms--; + GeomiDestroy(geom); + } + [DllImport("ode", CallingConvention = CallingConvention.Cdecl, EntryPoint = "dGeomDisable"), SuppressUnmanagedCodeSecurity] public static extern void GeomDisable(IntPtr geom); diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 74de2ee..e60b006 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1860,6 +1860,7 @@ namespace OpenSim.Region.Physics.OdePlugin int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); int nbodies = d.NTotalBodies; + int ngeoms = d.NTotalGeoms; // Finished with all sim stepping. If requested, dump world state to file for debugging. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? -- cgit v1.1 From 43a74d8481ff958d9642d2759915fdee6464ab1d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 10 Feb 2012 23:51:55 +0000 Subject: test on git --- OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs | 1 + OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 1 + 4 files changed, 4 insertions(+) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 6b323fb..0272f34 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 7718a29..b0dbe50 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs index 2b6bc59..e62746e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeApi.cs @@ -1,3 +1,4 @@ + /* * based on: * Ode.NET - .NET bindings for ODE diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index e60b006..fc59c32 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1,3 +1,4 @@ + /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. -- cgit v1.1 From b617411b972281529684bd8151a59899c81bfcf6 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 02:48:38 +0000 Subject: scale avatar push force with avatar density --- OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index c8f7c76..b0b91f6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -768,7 +768,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (pushforce) { m_pidControllerActive = false; - m_taintForce = force / _parent_scene.ODE_STEPSIZE; + // scale with odetime step and density + m_taintForce = force * m_density / _parent_scene.ODE_STEPSIZE / 28f; m_hasTaintForce = true; _parent_scene.AddPhysicsActorTaint(this); } -- cgit v1.1 From f415256e0b0d4b0191d52cb84090a0f1b0044ae9 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 03:25:17 +0000 Subject: Use mesh to estimate real center of prims if avaiable. Let sculpt map textures with alpha channel work. On linux J2DecodeCache folder must be deleted to remove bad entries. Corrently this can't be cached on linux (mono/ cairo/? problem) --- OpenSim/Region/Physics/Manager/IMesher.cs | 1 + OpenSim/Region/Physics/Meshing/Mesh.cs | 72 ++++++++++++++++++++++++- OpenSim/Region/Physics/Meshing/Meshmerizer.cs | 21 ++++++-- OpenSim/Region/Physics/Meshing/SculptMap.cs | 62 ++++++++++++--------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 6 +-- 5 files changed, 129 insertions(+), 33 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/Manager/IMesher.cs b/OpenSim/Region/Physics/Manager/IMesher.cs index 3a9ca1b..cc92484 100644 --- a/OpenSim/Region/Physics/Manager/IMesher.cs +++ b/OpenSim/Region/Physics/Manager/IMesher.cs @@ -65,5 +65,6 @@ namespace OpenSim.Region.Physics.Manager void releasePinned(); void Append(IMesh newMesh); void TransformLinear(float[,] matrix, float[] offset); + Vector3 GetCentroid(); } } diff --git a/OpenSim/Region/Physics/Meshing/Mesh.cs b/OpenSim/Region/Physics/Meshing/Mesh.cs index f781ff9..c715642 100644 --- a/OpenSim/Region/Physics/Meshing/Mesh.cs +++ b/OpenSim/Region/Physics/Meshing/Mesh.cs @@ -46,11 +46,36 @@ namespace OpenSim.Region.Physics.Meshing IntPtr m_indicesPtr = IntPtr.Zero; int m_indexCount = 0; public float[] m_normals; + Vector3 _centroid; + int _centroidDiv; + + private class vertexcomp : IEqualityComparer + { + public bool Equals(Vertex v1, Vertex v2) + { + if (v1.X == v2.X && v1.Y == v2.Y && v1.Z == v2.Z) + return true; + else + return false; + } + public int GetHashCode(Vertex v) + { + int a = v.X.GetHashCode(); + int b = v.Y.GetHashCode(); + int c = v.Z.GetHashCode(); + return (a << 16) ^ (b << 8) ^ c; + } + + } public Mesh() { - m_vertices = new Dictionary(); + vertexcomp vcomp = new vertexcomp(); + + m_vertices = new Dictionary(vcomp); m_triangles = new List(); + _centroid = Vector3.Zero; + _centroidDiv = 0; } public Mesh Clone() @@ -61,7 +86,8 @@ namespace OpenSim.Region.Physics.Meshing { result.Add(new Triangle(t.v1.Clone(), t.v2.Clone(), t.v3.Clone())); } - + result._centroid = _centroid; + result._centroidDiv = _centroidDiv; return result; } @@ -71,15 +97,57 @@ namespace OpenSim.Region.Physics.Meshing throw new NotSupportedException("Attempt to Add to a pinned Mesh"); // If a vertex of the triangle is not yet in the vertices list, // add it and set its index to the current index count + // vertex == seems broken + // skip colapsed triangles + if ((triangle.v1.X == triangle.v2.X && triangle.v1.Y == triangle.v2.Y && triangle.v1.Z == triangle.v2.Z) + || (triangle.v1.X == triangle.v3.X && triangle.v1.Y == triangle.v3.Y && triangle.v1.Z == triangle.v3.Z) + || (triangle.v2.X == triangle.v3.X && triangle.v2.Y == triangle.v3.Y && triangle.v2.Z == triangle.v3.Z) + ) + { + return; + } + + if (m_vertices.Count == 0) + { + _centroidDiv = 0; + _centroid = Vector3.Zero; + } + if (!m_vertices.ContainsKey(triangle.v1)) + { m_vertices[triangle.v1] = m_vertices.Count; + _centroid.X += triangle.v1.X; + _centroid.Y += triangle.v1.Y; + _centroid.Z += triangle.v1.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v2)) + { m_vertices[triangle.v2] = m_vertices.Count; + _centroid.X += triangle.v2.X; + _centroid.Y += triangle.v2.Y; + _centroid.Z += triangle.v2.Z; + _centroidDiv++; + } if (!m_vertices.ContainsKey(triangle.v3)) + { m_vertices[triangle.v3] = m_vertices.Count; + _centroid.X += triangle.v3.X; + _centroid.Y += triangle.v3.Y; + _centroid.Z += triangle.v3.Z; + _centroidDiv++; + } m_triangles.Add(triangle); } + public Vector3 GetCentroid() + { + if (_centroidDiv > 0) + return new Vector3(_centroid.X / _centroidDiv, _centroid.Y / _centroidDiv, _centroid.Z / _centroidDiv); + else + return Vector3.Zero; + } + public void CalcNormals() { int iTriangles = m_triangles.Count; diff --git a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs index f15e81b..ba461f7 100644 --- a/OpenSim/Region/Physics/Meshing/Meshmerizer.cs +++ b/OpenSim/Region/Physics/Meshing/Meshmerizer.cs @@ -74,6 +74,8 @@ namespace OpenSim.Region.Physics.Meshing #endif private bool cacheSculptMaps = true; + private bool cacheSculptAlphaMaps = true; + private string decodedSculptMapPath = null; private bool useMeshiesPhysicsMesh = false; @@ -87,7 +89,16 @@ namespace OpenSim.Region.Physics.Meshing IConfig mesh_config = config.Configs["Mesh"]; decodedSculptMapPath = start_config.GetString("DecodedSculptMapPath","j2kDecodeCache"); + cacheSculptMaps = start_config.GetBoolean("CacheSculptMaps", cacheSculptMaps); + + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + cacheSculptAlphaMaps = false; + } + else + cacheSculptAlphaMaps = cacheSculptMaps; + if(mesh_config != null) useMeshiesPhysicsMesh = mesh_config.GetBoolean("UseMeshiesPhysicsMesh", useMeshiesPhysicsMesh); @@ -268,15 +279,18 @@ namespace OpenSim.Region.Physics.Meshing { if (!GenerateCoordsAndFacesFromPrimSculptData(primName, primShape, size, lod, out coords, out faces)) return null; + // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // don't loose it + // primShape.SculptData = Utils.EmptyBytes; } +// primShape.SculptDataLoaded = true; } else { if (!GenerateCoordsAndFacesFromPrimShapeData(primName, primShape, size, lod, out coords, out faces)) return null; } - - // Remove the reference to any JPEG2000 sculpt data so it can be GCed + // keep compatible primShape.SculptData = Utils.EmptyBytes; int numCoords = coords.Count; @@ -482,7 +496,8 @@ namespace OpenSim.Region.Physics.Meshing //idata = CSJ2K.J2kImage.FromBytes(primShape.SculptData); - if (cacheSculptMaps) + if (cacheSculptMaps && (cacheSculptAlphaMaps || (((ImageFlags)(idata.Flags) & ImageFlags.HasAlpha) ==0))) + // don't cache images with alpha channel in linux since mono can't load them correctly) { try { idata.Save(decodedSculptFileName, ImageFormat.MemoryBmp); } catch (Exception e) { m_log.Error("[SCULPT]: unable to cache sculpt map " + decodedSculptFileName + " " + e.Message); } diff --git a/OpenSim/Region/Physics/Meshing/SculptMap.cs b/OpenSim/Region/Physics/Meshing/SculptMap.cs index 740424e..b3d9cb6 100644 --- a/OpenSim/Region/Physics/Meshing/SculptMap.cs +++ b/OpenSim/Region/Physics/Meshing/SculptMap.cs @@ -58,28 +58,24 @@ namespace PrimMesher if (bmW == 0 || bmH == 0) throw new Exception("SculptMap: bitmap has no data"); - int numLodPixels = lod * 2 * lod * 2; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + int numLodPixels = lod * lod; // (32 * 2)^2 = 64^2 pixels for default sculpt map image + bool smallMap = bmW * bmH <= numLodPixels; bool needsScaling = false; - bool smallMap = bmW * bmH <= lod * lod; - width = bmW; height = bmH; - while (width * height > numLodPixels) + while (width * height > numLodPixels * 4) { width >>= 1; height >>= 1; needsScaling = true; } - - try { if (needsScaling) - bm = ScaleImage(bm, width, height, - System.Drawing.Drawing2D.InterpolationMode.NearestNeighbor); + bm = ScaleImage(bm, width, height); } catch (Exception e) @@ -87,7 +83,7 @@ namespace PrimMesher throw new Exception("Exception in ScaleImage(): e: " + e.ToString()); } - if (width * height > lod * lod) + if (width * height > numLodPixels) { width >>= 1; height >>= 1; @@ -144,15 +140,17 @@ namespace PrimMesher int rowNdx, colNdx; int smNdx = 0; + for (rowNdx = 0; rowNdx < numRows; rowNdx++) { List row = new List(numCols); for (colNdx = 0; colNdx < numCols; colNdx++) { + if (mirror) - row.Add(new Coord(-(redBytes[smNdx] * pixScale - 0.5f), (greenBytes[smNdx] * pixScale - 0.5f), blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord(-((float)redBytes[smNdx] * pixScale - 0.5f), ((float)greenBytes[smNdx] * pixScale - 0.5f), (float)blueBytes[smNdx] * pixScale - 0.5f)); else - row.Add(new Coord(redBytes[smNdx] * pixScale - 0.5f, greenBytes[smNdx] * pixScale - 0.5f, blueBytes[smNdx] * pixScale - 0.5f)); + row.Add(new Coord((float)redBytes[smNdx] * pixScale - 0.5f, (float)greenBytes[smNdx] * pixScale - 0.5f, (float)blueBytes[smNdx] * pixScale - 0.5f)); ++smNdx; } @@ -161,23 +159,39 @@ namespace PrimMesher return rows; } - private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight, - System.Drawing.Drawing2D.InterpolationMode interpMode) + private Bitmap ScaleImage(Bitmap srcImage, int destWidth, int destHeight) { - Bitmap scaledImage = new Bitmap(srcImage, destWidth, destHeight); - scaledImage.SetResolution(96.0f, 96.0f); - - Graphics grPhoto = Graphics.FromImage(scaledImage); - grPhoto.InterpolationMode = interpMode; - grPhoto.DrawImage(srcImage, - new Rectangle(0, 0, destWidth, destHeight), - new Rectangle(0, 0, srcImage.Width, srcImage.Height), - GraphicsUnit.Pixel); + Bitmap scaledImage = new Bitmap(destWidth, destHeight, PixelFormat.Format24bppRgb); + + Color c; + float xscale = srcImage.Width / destWidth; + float yscale = srcImage.Height / destHeight; + + float sy = 0.5f; + for (int y = 0; y < destHeight; y++) + { + float sx = 0.5f; + for (int x = 0; x < destWidth; x++) + { + try + { + c = srcImage.GetPixel((int)(sx), (int)(sy)); + scaledImage.SetPixel(x, y, Color.FromArgb(c.R, c.G, c.B)); + } + catch (IndexOutOfRangeException) + { + } - grPhoto.Dispose(); + sx += xscale; + } + sy += yscale; + } + srcImage.Dispose(); return scaledImage; } + + } + } -} #endif diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index b0dbe50..073c37f 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -1056,9 +1055,8 @@ namespace OpenSim.Region.Physics.OdePlugin return false; } -// primOOBoffset = mesh.GetCentroid(); -// hasOOBoffsetFromMesh = true; - hasOOBoffsetFromMesh = false; + primOOBoffset = mesh.GetCentroid(); + hasOOBoffsetFromMesh = true; _triMeshData = d.GeomTriMeshDataCreate(); -- cgit v1.1 From 33a9f0f1c5aa4cb6b49399dfe6b309b59f77266e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 04:42:45 +0000 Subject: a bit better vertical attractor and banking --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 35 ++++++++++++++-------- 1 file changed, 23 insertions(+), 12 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 0272f34..8999621 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -161,7 +160,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_angularMotorDecayTimescale = pValue * invtimestep; break; @@ -210,7 +211,9 @@ namespace OpenSim.Region.Physics.OdePlugin m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < 0.5f) pValue = 0.5f; else if (pValue > 120) pValue = 120; m_linearMotorDecayTimescale = pValue * invtimestep; break; @@ -788,38 +791,46 @@ namespace OpenSim.Region.Physics.OdePlugin } } + float r; + float p; + float y; + rotq.GetEulerAngles(out r, out p, out y); + // vertical atractor if (m_verticalAttractionTimescale < 300) { float roll; float pitch; - GetRollPitch(rotq, out roll, out pitch); + GetRollPitch(irotq, out roll, out pitch); float ftmp = 1.0f / m_verticalAttractionTimescale / m_verticalAttractionTimescale / _pParentScene.ODE_STEPSIZE; float ftmp2 = m_verticalAttractionEfficiency / _pParentScene.ODE_STEPSIZE; if (Math.Abs(roll) > 0.01) // roll { - torque.X -= roll * ftmp + curLocalAngVel.X * ftmp2; + torque.X -= -roll * ftmp + curLocalAngVel.X * ftmp2; } if (Math.Abs(pitch) > 0.01 && ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) == 0)) // pitch { - torque.Y -= pitch * ftmp + curLocalAngVel.Y * ftmp2; + torque.Y -= -pitch * ftmp + curLocalAngVel.Y * ftmp2; } - if (m_bankingEfficiency != 0 && Math.Abs(roll) < 0.01) + if (m_bankingEfficiency != 0 && Math.Abs(roll) > 0.01) { - float broll = -roll * m_bankingEfficiency; ; + float broll = roll * m_bankingEfficiency; ; if (m_bankingMix != 0) { - float vfact = m_bankingMix * Math.Abs(curLocalVel.X) / 10.0f; - if (vfact < m_bankingMix) + float vfact = Math.Abs(curLocalVel.X) / 10.0f; + if (vfact > 1.0f) vfact = 1.0f; + if (curLocalVel.X >= 0) broll *= ((1 - m_bankingMix) + vfact); + else + broll *= -((1 - m_bankingMix) + vfact); } - - torque.Z += (broll - curLocalAngVel.Z) / m_bankingTimescale; + broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; + torque.Z += broll; } } -- cgit v1.1 From 3f9c390b4d1e13057b11ef810f19fde7c6722fca Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 14:29:36 +0000 Subject: changes to vehicle banking --- .../Region/Physics/UbitOdePlugin/ODEDynamics.cs | 92 ++++++---------------- 1 file changed, 23 insertions(+), 69 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs index 8999621..80218e7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.cs @@ -38,6 +38,8 @@ * settings use. */ +// Ubit 2012 + using System; using System.Collections.Generic; using System.Reflection; @@ -57,24 +59,15 @@ namespace OpenSim.Region.Physics.OdePlugin 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 Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: // HOVER_TERRAIN_ONLY // HOVER_GLOBAL_HEIGHT @@ -84,7 +77,6 @@ namespace OpenSim.Region.Physics.OdePlugin // 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 @@ -116,7 +108,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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. @@ -126,9 +117,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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 @@ -271,7 +259,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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; @@ -575,7 +562,6 @@ namespace OpenSim.Region.Physics.OdePlugin return vec; } - public static void GetRollPitch(Quaternion rot, out float roll, out float pitch) { // assuming rot is normalised @@ -617,23 +603,23 @@ namespace OpenSim.Region.Physics.OdePlugin d.Vector3 dvtmp; Vector3 tmpV; + Vector3 curVel; // velocity in world + Vector3 curAngVel; // angular velocity in world + Vector3 force = Vector3.Zero; // actually linear aceleration until mult by mass in world frame + Vector3 torque = Vector3.Zero;// actually angular aceleration until mult by Inertia in vehicle frame + d.Vector3 dtorque = new d.Vector3(); + dvtmp = d.BodyGetLinearVel(Body); - 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(); + Vector3 curLocalAngVel = curAngVel * irotq; // current angular velocity in local // linear motor if (m_lmEfect > 0.01 && m_linearMotorTimescale < 1000) @@ -791,11 +777,6 @@ namespace OpenSim.Region.Physics.OdePlugin } } - float r; - float p; - float y; - rotq.GetEulerAngles(out r, out p, out y); - // vertical atractor if (m_verticalAttractionTimescale < 300) { @@ -830,7 +811,17 @@ namespace OpenSim.Region.Physics.OdePlugin broll *= -((1 - m_bankingMix) + vfact); } broll = (broll - curLocalAngVel.Z) / m_bankingTimescale; - torque.Z += broll; + // torque.Z += broll; + + // make z rot be in world Z not local as seems to be in sl + tmpV.X = 0; + tmpV.Y = 0; + tmpV.Z = broll; + tmpV *= irotq; + + torque.X += tmpV.X; + torque.Y += tmpV.Y; + torque.Z += tmpV.Z; } } @@ -853,43 +844,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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); -*/ } } } -- cgit v1.1 From d4e28ed113659fa0e497aa0adedc31d1deddf2ec Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 15:18:13 +0000 Subject: vehicle changes done by simulation thread and not calling one --- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 325 ++++++++++++++--------- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 7 +- 2 files changed, 200 insertions(+), 132 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 073c37f..53a39d7 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -25,7 +25,7 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -/* Revision 2011 by Ubit Umarov +/* Revision 2011/12 by Ubit Umarov * * */ @@ -190,7 +190,7 @@ namespace OpenSim.Region.Physics.OdePlugin public ODEDynamics m_vehicle; internal int m_material = (int)Material.Wood; - protected ContactData primContactData = new ContactData { mu = 0f, bounce = 0.1f}; + 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. @@ -209,7 +209,7 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.Physical, value); } } - + public override bool Building // this is not reliable for internal use { get { return m_building; } @@ -225,23 +225,10 @@ namespace OpenSim.Region.Physics.OdePlugin { 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; + return primContactData; } } - + public override int PhysicsActorType { get { return (int)ActorTypes.Prim; } @@ -276,11 +263,11 @@ namespace OpenSim.Region.Physics.OdePlugin { set { - if(value) + if (value) m_isSelected = value; AddChange(changes.Selected, value); } - } + } public override bool Flying { @@ -400,10 +387,9 @@ namespace OpenSim.Region.Physics.OdePlugin } } - public override void SetVolumeDetect(int param) { - AddChange(changes.VolumeDtc,(param != 0)); + AddChange(changes.VolumeDtc, (param != 0)); } public override Vector3 GeometricCenter @@ -483,14 +469,6 @@ namespace OpenSim.Region.Physics.OdePlugin // 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 @@ -498,7 +476,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { AddChange(changes.Velocity, value); -// _velocity = value; + // _velocity = value; } else @@ -548,7 +526,7 @@ namespace OpenSim.Region.Physics.OdePlugin { get { - if (givefakeori>0) + if (givefakeori > 0) return fakeori; else @@ -603,7 +581,7 @@ namespace OpenSim.Region.Physics.OdePlugin } } - + public override float Buoyancy { get { return m_buoyancy; } @@ -652,6 +630,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override int VehicleType { + // we may need to put a fake on this get { if (m_vehicle == null) @@ -661,44 +640,32 @@ namespace OpenSim.Region.Physics.OdePlugin } 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); + AddChange(changes.VehicleType, 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); + strVehicleFloatParam fp = new strVehicleFloatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleFloatParam, fp); } 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); + strVehicleVectorParam fp = new strVehicleVectorParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); } - public override void VehicleRotationParam(int param, Quaternion rotation) + public override void VehicleRotationParam(int param, Quaternion value) { - if (m_vehicle == null) - return; - m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleQuatParam fp = new strVehicleQuatParam(); + fp.param = param; + fp.value = value; + AddChange(changes.VehicleVectorParam, fp); } public override void VehicleFlags(int param, bool remove) @@ -829,7 +796,7 @@ namespace OpenSim.Region.Physics.OdePlugin return false; } - + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) { @@ -866,7 +833,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (size.Z <= 0) size.Z = 0.01f; _size = size; - + if (!QuaternionIsFinite(rotation)) { @@ -1023,7 +990,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null) { OdePrim parent = (OdePrim)_parent; - parent.ChildDelink(this,false); + parent.ChildDelink(this, false); } } else @@ -1093,23 +1060,23 @@ namespace OpenSim.Region.Physics.OdePlugin _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); - } - } - */ + /* + 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. /// @@ -1128,10 +1095,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent_scene.needsMeshing(_pbs)) { - haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims + haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims } - if(!haveMesh) + if (!haveMesh) { if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 && _size.X == _size.Y && _size.Y == _size.Z) @@ -1186,7 +1153,7 @@ namespace OpenSim.Region.Physics.OdePlugin // catch (System.AccessViolationException) catch (Exception e) { - m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name,e); + m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); } prim_geom = IntPtr.Zero; @@ -1526,7 +1493,7 @@ namespace OpenSim.Region.Physics.OdePlugin } #region Mass Calculation - + private float CalculatePrimVolume() { float volume = _size.X * _size.Y * _size.Z; // default @@ -1885,7 +1852,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (prm.prim_geom != IntPtr.Zero) d.GeomSetBody(prm.prim_geom, IntPtr.Zero); - if(prm.Body != prim.Body) + if (prm.Body != prim.Body) prm.DestroyBody(); // don't loose bodies around prm.Body = IntPtr.Zero; } @@ -2057,21 +2024,21 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } -// m_building = false; // REMOVE THIS LATER + // m_building = false; // REMOVE THIS LATER if (m_isphysical && Body == IntPtr.Zero) { -/* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - { - */ - MakeBody(); -// } + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + { + */ + MakeBody(); + // } } } @@ -2114,7 +2081,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (NewParent != _parent) { - (_parent as OdePrim).ChildDelink(this,false); // for now... + (_parent as OdePrim).ChildDelink(this, false); // for now... childPrim = false; if (NewParent != null) @@ -2130,7 +2097,7 @@ namespace OpenSim.Region.Physics.OdePlugin private void Stop() { - if(!childPrim) + if (!childPrim) { m_force = Vector3.Zero; m_forceacc = Vector3.Zero; @@ -2138,7 +2105,7 @@ namespace OpenSim.Region.Physics.OdePlugin _torque = Vector3.Zero; _velocity = Vector3.Zero; _acceleration = Vector3.Zero; - m_rotationalVelocity = Vector3.Zero; + m_rotationalVelocity = Vector3.Zero; _target_velocity = Vector3.Zero; if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Stop(); @@ -2150,7 +2117,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetTorque(Body, 0f, 0f, 0f); d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetAngularVel(Body, 0f, 0f, 0f); - + } } @@ -2236,7 +2203,7 @@ namespace OpenSim.Region.Physics.OdePlugin givefakepos--; if (givefakepos < 0) givefakepos = 0; -// changeSelectedStatus(); + // changeSelectedStatus(); resetCollisionAccounting(); } @@ -2381,17 +2348,17 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childPrim) { - if (NewStatus) + if (NewStatus) { if (Body == IntPtr.Zero) { -/* - if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ + /* + if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ { MakeBody(); } @@ -2402,13 +2369,13 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != IntPtr.Zero) { // UpdateChildsfromgeom(); -/* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) - { - changeShape(_pbs); - } - else - */ - DestroyBody(); + /* if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) + { + changeShape(_pbs); + } + else + */ + DestroyBody(); } } } @@ -2417,7 +2384,7 @@ namespace OpenSim.Region.Physics.OdePlugin } private void changeprimsizeshape() - { + { OdePrim parent = (OdePrim)_parent; bool chp = childPrim; @@ -2609,6 +2576,57 @@ namespace OpenSim.Region.Physics.OdePlugin } } + private void changeVehicleType(int value) + { + if (m_vehicle == null) + { + if (value != (int)Vehicle.TYPE_NONE) + { + m_vehicle = new ODEDynamics(this); + m_vehicle.ProcessTypeChange((Vehicle)value); + } + } + else + m_vehicle.ProcessTypeChange((Vehicle)value); + } + + private void changeVehicleFloatParam(strVehicleFloatParam fp) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleVectorParam(strVehicleVectorParam vp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleRotationParam(strVehicleQuatParam qp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + + private void changeVehicleFlags(strVehicleBoolParam bp) + { + if (m_vehicle == null) + return; + m_vehicle.ProcessVehicleFlags(bp.param, bp.value); + if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) + d.BodyEnable(Body); + } + #endregion public void Move() @@ -2616,10 +2634,10 @@ namespace OpenSim.Region.Physics.OdePlugin 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 + // 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; @@ -2631,13 +2649,13 @@ namespace OpenSim.Region.Physics.OdePlugin } else { - float m_mass = _mass; - + 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 @@ -2795,7 +2813,7 @@ namespace OpenSim.Region.Physics.OdePlugin 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) { @@ -2871,10 +2889,10 @@ namespace OpenSim.Region.Physics.OdePlugin bool lastZeroFlag = _zeroFlag; d.Vector3 lpos; - d.GeomCopyPosition(prim_geom,out lpos); // root position that is seem by rest of simulator - + 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 + if (lpos.X < 0f || lpos.X > _parent_scene.WorldExtents.X || lpos.Y < 0f || lpos.Y > _parent_scene.WorldExtents.Y ) { @@ -3137,10 +3155,10 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null) { OdePrim parent = (OdePrim)_parent; - parent.ChildRemove(this,false); + parent.ChildRemove(this, false); } else - ChildRemove(this,false); + ChildRemove(this, false); RemoveGeom(); m_targetSpace = IntPtr.Zero; @@ -3177,12 +3195,12 @@ namespace OpenSim.Region.Physics.OdePlugin changevelocity((Vector3)arg); break; -// case changes.Acceleration: -// changeacceleration((Vector3)arg); -// break; -// case changes.AngVelocity: -// changeangvelocity((Vector3)arg); -// break; + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; case changes.Force: changeForce((Vector3)arg); @@ -3209,7 +3227,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.Shape: - changeShape((PrimitiveBaseShape) arg); + changeShape((PrimitiveBaseShape)arg); break; case changes.CollidesWater: @@ -3229,13 +3247,33 @@ namespace OpenSim.Region.Physics.OdePlugin break; case changes.disabled: - changeDisable((bool) arg); + changeDisable((bool)arg); break; case changes.building: changeBuilding((bool)arg); break; + case changes.VehicleType: + changeVehicleType((int)arg); + break; + + case changes.VehicleFlags: + changeVehicleFlags((strVehicleBoolParam) arg); + break; + + case changes.VehicleFloatParam: + changeVehicleFloatParam((strVehicleFloatParam) arg); + break; + + case changes.VehicleVectorParam: + changeVehicleVectorParam((strVehicleVectorParam) arg); + break; + + case changes.VehicleRotationParam: + changeVehicleRotationParam((strVehicleQuatParam) arg); + break; + case changes.Null: donullchange(); break; @@ -3251,5 +3289,30 @@ namespace OpenSim.Region.Physics.OdePlugin { _parent_scene.AddChange(this, what, arg); } + + + private struct strVehicleBoolParam + { + public int param; + public bool value; + } + + private struct strVehicleFloatParam + { + public int param; + public float value; + } + + private struct strVehicleQuatParam + { + public int param; + public Quaternion value; + } + + private struct strVehicleVectorParam + { + public int param; + public Vector3 value; + } } -} +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index fc59c32..86385bf 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -1,4 +1,3 @@ - /* * Copyright (c) Contributors, http://opensimulator.org/ * See CONTRIBUTORS.TXT for a full list of copyright holders. @@ -137,6 +136,12 @@ namespace OpenSim.Region.Physics.OdePlugin disabled, building, + VehicleType, + VehicleFloatParam, + VehicleVectorParam, + VehicleRotationParam, + VehicleFlags, + Null //keep this last used do dim the methods array. does nothing but pulsing the prim } -- cgit v1.1 From 022ae33ed5da0b1d9b56694bcfbf2f1a9e18b36e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 17:35:38 +0000 Subject: UbitODE: replace 'taints' by 'changes' for avatars also. This provides better time order with changes in prims. --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 563 +++++++++++++-------- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 14 +- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 51 +- 3 files changed, 378 insertions(+), 250 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index b0b91f6..cf7fdca 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -26,7 +26,7 @@ */ -// Revision by Ubit 2011 +// Revision by Ubit 2011/12 using System; using System.Collections.Generic; @@ -95,21 +95,12 @@ namespace OpenSim.Region.Physics.OdePlugin 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; @@ -151,15 +142,13 @@ namespace OpenSim.Region.Physics.OdePlugin { m_uuid = UUID.Random(); - m_hasTaintPosition = false; - if (pos.IsFinite()) { - if (pos.Z > 9999999f) + if (pos.Z > 99999f) { pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - if (pos.Z < -90000f) + if (pos.Z < -100f) // shouldn't this be 0 ? { pos.Z = parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } @@ -187,15 +176,12 @@ namespace OpenSim.Region.Physics.OdePlugin 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; + + AddChange(changes.Add, null); } public override int PhysicsActorType @@ -402,16 +388,11 @@ namespace OpenSim.Region.Physics.OdePlugin { value.Z = _parent_scene.GetTerrainHeightAtXY(127, 127) + 5; } - if (value.Z < -90000f) + if (value.Z < -100f) { 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); + AddChange(changes.Position, value); } else { @@ -440,11 +421,7 @@ namespace OpenSim.Region.Physics.OdePlugin { 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); + AddChange(changes.Size, value); } else { @@ -460,129 +437,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// /// - // 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. @@ -705,8 +559,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (value.IsFinite()) { - m_pidControllerActive = true; - _target_velocity = value; + AddChange(changes.Velocity, value); } else { @@ -738,9 +591,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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); } } @@ -767,18 +617,11 @@ namespace OpenSim.Region.Physics.OdePlugin { if (pushforce) { - m_pidControllerActive = false; - // scale with odetime step and density - m_taintForce = force * m_density / _parent_scene.ODE_STEPSIZE / 28f; - m_hasTaintForce = true; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Force, force * m_density / _parent_scene.ODE_STEPSIZE / 28f); } else { - m_pidControllerActive = true; - _target_velocity.X += force.X; - _target_velocity.Y += force.Y; - _target_velocity.Z += force.Z; + AddChange(changes.Velocity, force); } } else @@ -798,6 +641,128 @@ namespace OpenSim.Region.Physics.OdePlugin } + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + Shell = d.CreateCapsule(_parent_scene.ActiveSpace, CAPSULE_RADIUS, CAPSULE_LENGTH); + + d.GeomSetCategoryBits(Shell, (int)m_collisionCategories); + d.GeomSetCollideBits(Shell, (int)m_collisionFlags); + + d.MassSetCapsule(out ShellMass, m_density, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + + m_mass = ShellMass.mass; // update mass + + // rescale PID parameters + PID_D = _parent_scene.avPIDD; + PID_P = _parent_scene.avPIDP; + + // rescale PID parameters so that this aren't affected by mass + // and so don't get unstable for some masses + // also scale by ode time step so you don't need to refix them + + PID_D /= 50 * 80; //scale to original mass of around 80 and 50 ODE fps + PID_D *= m_mass / _parent_scene.ODE_STEPSIZE; + PID_P /= 50 * 80; + PID_P *= m_mass / _parent_scene.ODE_STEPSIZE; + + Body = d.BodyCreate(_parent_scene.world); + + d.BodySetAutoDisableFlag(Body, false); + d.BodySetPosition(Body, npositionX, npositionY, npositionZ); + + _position.X = npositionX; + _position.Y = npositionY; + _position.Z = npositionZ; + + d.BodySetMass(Body, ref ShellMass); + d.GeomSetBody(Shell, Body); + + // The purpose of the AMotor here is to keep the avatar's physical + // surrogate from rotating while moving + Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); + d.JointAttach(Amotor, Body, IntPtr.Zero); + + d.JointSetAMotorMode(Amotor, 0); + d.JointSetAMotorNumAxes(Amotor, 3); + d.JointSetAMotorAxis(Amotor, 0, 0, 1, 0, 0); + d.JointSetAMotorAxis(Amotor, 1, 0, 0, 1, 0); + d.JointSetAMotorAxis(Amotor, 2, 0, 0, 0, 1); + + d.JointSetAMotorAngle(Amotor, 0, 0); + d.JointSetAMotorAngle(Amotor, 1, 0); + d.JointSetAMotorAngle(Amotor, 2, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); // make it HARD + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP2, 0.8f); + d.JointSetAMotorParam(Amotor, (int)dParam.StopERP3, 0.8f); + + // These lowstops and high stops are effectively (no wiggle room) + d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -1e-5f); + d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 1e-5f); + + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel2, 0); + d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel3, 0); + + d.JointSetAMotorParam(Amotor, (int)dParam.FMax, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax2, 5e6f); + d.JointSetAMotorParam(Amotor, (int)dParam.FMax3, 5e6f); + } + + /// + /// Destroys the avatar body and geom + + private void AvatarGeomAndBodyDestroy() + { + // Kill the Amotor + if (Amotor != IntPtr.Zero) + { + d.JointDestroy(Amotor); + Amotor = IntPtr.Zero; + } + + if (Body != IntPtr.Zero) + { + //kill the body + d.BodyDestroy(Body); + Body = IntPtr.Zero; + } + + //kill the Geometry + if (Shell != IntPtr.Zero) + { + _parent_scene.geom_name_map.Remove(Shell); + _parent_scene.waitForSpaceUnlock(_parent_scene.ActiveSpace); + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + Shell = IntPtr.Zero; + } + } + /// /// Called from Simulate /// This is the avatar's movement control + PID Controller @@ -1136,8 +1101,7 @@ namespace OpenSim.Region.Physics.OdePlugin /// public void Destroy() { - m_tainted_isPhysical = false; - _parent_scene.AddPhysicsActorTaint(this); + AddChange(changes.Remove, null); } public override void CrossingFailure() @@ -1207,12 +1171,11 @@ namespace OpenSim.Region.Physics.OdePlugin return m_haseventsubscription; } - public void ProcessTaints(float timestep) + private void changePhysicsStatus(bool NewStatus) { - - if (m_tainted_isPhysical != m_isPhysical) + if (NewStatus != m_isPhysical) { - if (m_tainted_isPhysical) + if (NewStatus) { // Create avatar capsule and related ODE data if ((Shell != IntPtr.Zero)) @@ -1237,60 +1200,246 @@ namespace OpenSim.Region.Physics.OdePlugin AvatarGeomAndBodyDestroy(); } - m_isPhysical = m_tainted_isPhysical; + m_isPhysical = NewStatus; } + } + + private void changeAdd() + { + changePhysicsStatus(true); + } - if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + private void changeRemove() + { + changePhysicsStatus(false); + } + + private void changeShape(PrimitiveBaseShape arg) + { + } + + private void changeSize(Vector3 Size) + { + if (Size.IsFinite()) { - if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + float caplen = Size.Z; + + caplen = caplen * 1.15f - CAPSULE_RADIUS * 2.0f; + + if (caplen != CAPSULE_LENGTH) { - AvatarGeomAndBodyDestroy(); + if (Shell != IntPtr.Zero && Body != IntPtr.Zero && Amotor != IntPtr.Zero) + { + AvatarGeomAndBodyDestroy(); - m_pidControllerActive = true; + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = caplen; - float prevCapsule = CAPSULE_LENGTH; - CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; - - AvatarGeomAndBodyCreation(_position.X, _position.Y, - _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + AvatarGeomAndBodyCreation(_position.X, _position.Y, + _position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); - Velocity = Vector3.Zero; + 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 " : "")); + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == IntPtr.Zero ? "Shell " : "") + + (Body == IntPtr.Zero ? "Body " : "") + + (Amotor == IntPtr.Zero ? "Amotor " : "")); + } } + + m_pidControllerActive = true; + } + else + { + m_log.Warn("[PHYSICS]: Got a NaN Size from Scene on a Character"); } + } - if (m_hasTaintPosition) + private void changePosition( Vector3 newPos) { if (Body != IntPtr.Zero) - d.BodySetPosition(Body, m_taintPosition.X, m_taintPosition.Y, m_taintPosition.Z); + d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z); + _position = newPos; + } + + private void changeOrientation(Quaternion newOri) + { + } + + private void changeVelocity(Vector3 newVel) + { + m_pidControllerActive = true; + _target_velocity = newVel; + } - _position.X = m_taintPosition.X; - _position.Y = m_taintPosition.Y; - _position.Z = m_taintPosition.Z; - m_hasTaintPosition = false; + private void changeSetTorque(Vector3 newTorque) + { + } + + private void changeAddForce(Vector3 newForce) + { + } + + private void changeAddAngularForce(Vector3 arg) + { + } + + private void changeAngularLock(Vector3 arg) + { + } + + private void changeFloatOnWater(bool arg) + { + } + + private void changeVolumedetetion(bool arg) + { + } + + private void changeSelectedStatus(bool arg) + { + } + + private void changeDisable(bool arg) + { + } + + private void changeBuilding(bool arg) + { + } + + private void changeForce(Vector3 newForce) + { + m_pidControllerActive = false; + if (Body != IntPtr.Zero) + { + if (newForce.X != 0f || newForce.Y != 0f || newForce.Z != 0) + d.BodyAddForce(Body, newForce.X, newForce.Y, newForce.Z); + } + } + + private void donullchange() + { + } + + public bool DoAChange(changes what, object arg) + { + if (Shell == IntPtr.Zero && what != changes.Add && what != changes.Remove) + { + return false; } - if (m_hasTaintForce) + // nasty switch + switch (what) { - 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; - } + case changes.Add: + changeAdd(); + break; + case changes.Remove: + changeRemove(); + break; + + case changes.Position: + changePosition((Vector3)arg); + break; + + case changes.Orientation: + changeOrientation((Quaternion)arg); + break; + + case changes.PosOffset: + donullchange(); + break; + + case changes.OriOffset: + donullchange(); + break; + + case changes.Velocity: + changeVelocity((Vector3)arg); + break; + + // case changes.Acceleration: + // changeacceleration((Vector3)arg); + // break; + // case changes.AngVelocity: + // changeangvelocity((Vector3)arg); + // break; + + case changes.Force: + changeForce((Vector3)arg); + break; + + case changes.Torque: + changeSetTorque((Vector3)arg); + break; + + case changes.AddForce: + changeAddForce((Vector3)arg); + break; + + case changes.AddAngForce: + changeAddAngularForce((Vector3)arg); + break; + + case changes.AngLock: + changeAngularLock((Vector3)arg); + break; + + case changes.Size: + changeSize((Vector3)arg); + break; +/* not in use for now + case changes.Shape: + changeShape((PrimitiveBaseShape)arg); + break; + + case changes.CollidesWater: + changeFloatOnWater((bool)arg); + break; + + case changes.VolumeDtc: + changeVolumedetetion((bool)arg); + break; + + case changes.Physical: + changePhysicsStatus((bool)arg); + break; + + case changes.Selected: + changeSelectedStatus((bool)arg); + break; + + case changes.disabled: + changeDisable((bool)arg); + break; + + case changes.building: + changeBuilding((bool)arg); + break; +*/ + case changes.Null: + donullchange(); + break; + + default: + donullchange(); + break; } + return false; + } + public void AddChange(changes what, object arg) + { + _parent_scene.AddChange((PhysicsActor)this, what, arg); } + internal void AddCollisionFrameTime(int p) { // protect it from overflow crashing diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 53a39d7..078adeb 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -668,13 +668,14 @@ namespace OpenSim.Region.Physics.OdePlugin AddChange(changes.VehicleVectorParam, fp); } - public override void VehicleFlags(int param, bool remove) + public override void VehicleFlags(int param, bool value) { if (m_vehicle == null) return; - m_vehicle.ProcessVehicleFlags(param, remove); - if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) - d.BodyEnable(Body); + strVehicleBoolParam bp = new strVehicleBoolParam(); + bp.param = param; + bp.value = value; + AddChange(changes.VehicleFlags, bp); } public void SetAcceleration(Vector3 accel) @@ -2374,8 +2375,9 @@ namespace OpenSim.Region.Physics.OdePlugin changeShape(_pbs); } else - */ + */ DestroyBody(); + Stop(); } } } @@ -3287,7 +3289,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void AddChange(changes what, object arg) { - _parent_scene.AddChange(this, what, arg); + _parent_scene.AddChange((PhysicsActor) this, what, arg); } diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 86385bf..2dfcca6 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -147,7 +147,7 @@ namespace OpenSim.Region.Physics.OdePlugin public struct ODEchangeitem { - public OdePrim prim; + public PhysicsActor actor; public OdeCharacter character; public changes what; public Object arg; @@ -187,7 +187,6 @@ namespace OpenSim.Region.Physics.OdePlugin public float gravityy = 0f; public float gravityz = -9.8f; - private float waterlevel = 0f; private int framecount = 0; @@ -1558,23 +1557,15 @@ namespace OpenSim.Region.Physics.OdePlugin 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 + /// Called to queue a change to a actor /// to use in place of old taint mechanism so changes do have a time sequence /// - public void AddChange(OdeCharacter character, changes what, Object arg) + + public void AddChange(PhysicsActor actor, changes what, Object arg) { ODEchangeitem item = new ODEchangeitem(); - item.character = character; + item.actor = actor; item.what = what; item.arg = arg; ChangesQueue.Enqueue(item); @@ -1687,25 +1678,6 @@ namespace OpenSim.Region.Physics.OdePlugin // 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) @@ -1716,14 +1688,19 @@ namespace OpenSim.Region.Physics.OdePlugin while(ChangesQueue.Dequeue(out item)) { - if (item.prim != null) + if (item.actor != null) { try { - if (item.prim.DoAChange(item.what, item.arg)) - RemovePrimThreadLocked(item.prim); + if (item.actor is OdeCharacter) + ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); + else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) + RemovePrimThreadLocked((OdePrim)item.actor); } - catch { }; + catch + { + m_log.Warn("[PHYSICS]: doChange failed for a actor"); + }; } ttmp = Util.EnvironmentTickCountSubtract(ttmpstart); if (ttmp > 20) -- cgit v1.1 From 09f6647aa37be99191263a20a98a5b992f62a3de Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 17:48:49 +0000 Subject: scenepresence change! in standup() give avatar a physical actor after deciding the new position. This reduces a bit the odds of it being still coliding with object. --- OpenSim/Region/Framework/Scenes/ScenePresence.cs | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs index c7c90da..9100c29 100644 --- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs +++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs @@ -1814,8 +1814,11 @@ namespace OpenSim.Region.Framework.Scenes // m_log.DebugFormat("[SCENE PRESENCE]: StandUp() for {0}", Name); SitGround = false; + +/* move this down so avatar gets physical in the new position and not where it is siting if (PhysicsActor == null) AddToPhysicalScene(false); + */ if (ParentID != 0) { @@ -1850,6 +1853,10 @@ namespace OpenSim.Region.Framework.Scenes ParentPosition = Vector3.Zero; ParentID = 0; + + if (PhysicsActor == null) + AddToPhysicalScene(false); + SendAvatarDataToAllAgents(); m_requestedSitTargetID = 0; @@ -1857,6 +1864,9 @@ namespace OpenSim.Region.Framework.Scenes part.ParentGroup.TriggerScriptChangedEvent(Changed.LINK); } + else if (PhysicsActor == null) + AddToPhysicalScene(false); + Animator.TrySetMovementAnimation("STAND"); } -- cgit v1.1 From bcf59a574f757fd8a79c3fc3bb61fab87da8c1f1 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:12:04 +0000 Subject: remove forgotten taints code --- OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 32 ------------------------ 1 file changed, 32 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 2dfcca6..233267e 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -228,10 +228,6 @@ namespace OpenSim.Region.Physics.OdePlugin 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(); /// @@ -1579,34 +1575,6 @@ namespace OpenSim.Region.Physics.OdePlugin /// 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); - } - } - } } /// -- cgit v1.1 From f168fefb790e1c951a7a59cafa4d2cb2520e4e06 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:14:27 +0000 Subject: removed outdated coments. --- .../Physics/UbitOdePlugin/ODEDynamics.c_comments | 630 --------------------- 1 file changed, 630 deletions(-) delete mode 100644 OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments deleted file mode 100644 index 1060aa6..0000000 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEDynamics.c_comments +++ /dev/null @@ -1,630 +0,0 @@ -/* - * 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 - } -} -- cgit v1.1 From 6fd6919a0bcdfd2d15d0b7a7aa392837de759114 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 11 Feb 2012 19:25:41 +0000 Subject: remove drawstuff from ubitode --- .../Region/Physics/UbitOdePlugin/ODECharacter.cs | 2 +- OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 3 - OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs | 153 --------------------- OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs | 99 ------------- 4 files changed, 1 insertion(+), 256 deletions(-) delete mode 100644 OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs index cf7fdca..793e281 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs @@ -126,7 +126,7 @@ namespace OpenSim.Region.Physics.OdePlugin public IntPtr Shell = IntPtr.Zero; public IntPtr Amotor = IntPtr.Zero; public d.Mass ShellMass; - public bool collidelock = false; +// public bool collidelock = false; private bool m_haseventsubscription = false; public int m_eventsubscription = 0; diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs index 078adeb..3b7f562 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs @@ -2025,9 +2025,6 @@ namespace OpenSim.Region.Physics.OdePlugin SetInStaticSpace(this); } - // m_building = false; // REMOVE THIS LATER - - if (m_isphysical && Body == IntPtr.Zero) { /* diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs index 233267e..56f3786 100644 --- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs +++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs @@ -25,7 +25,6 @@ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -//#define USE_DRAWSTUFF //#define SPAM using System; @@ -38,9 +37,6 @@ 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; @@ -366,31 +362,11 @@ namespace OpenSim.Region.Physics.OdePlugin //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) @@ -2560,134 +2536,5 @@ namespace OpenSim.Region.Physics.OdePlugin } 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 deleted file mode 100644 index aefad3a..0000000 --- a/OpenSim/Region/Physics/UbitOdePlugin/drawstuff.cs +++ /dev/null @@ -1,99 +0,0 @@ -/* - * Copyright ODE - * Ode.NET - .NET bindings for ODE - * Jason Perkins (starkos@industriousone.com) - * Licensed under the New BSD - * Part of the OpenDynamicsEngine -Open Dynamics Engine -Copyright (c) 2001-2007, Russell L. Smith. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - -Redistributions of source code must retain the above copyright notice, -this list of conditions and the following disclaimer. - -Redistributions in binary form must reproduce the above copyright notice, -this list of conditions and the following disclaimer in the documentation -and/or other materials provided with the distribution. - -Neither the names of ODE's copyright owner nor the names of its -contributors may be used to endorse or promote products derived from -this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED -TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * - */ - -using System; -using System.Runtime.InteropServices; -using 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 From a758abaa9fa7250d1b61bcd906ca12539307851d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 17:08:33 +0000 Subject: try to make crossings work better. chode no longer prevents crossings i hope --- .../EntityTransfer/EntityTransferModule.cs | 33 +++++++------ .../Region/Framework/Scenes/SceneObjectGroup.cs | 9 ++-- OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 6 +-- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 56 +++++++++++++++++++--- 4 files changed, 77 insertions(+), 27 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs b/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs index 9a6dfe1..557555d 100644 --- a/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs +++ b/OpenSim/Region/CoreModules/Framework/EntityTransfer/EntityTransferModule.cs @@ -1655,35 +1655,38 @@ namespace OpenSim.Region.CoreModules.Framework.EntityTransfer } // Offset the positions for the new region across the border - Vector3 oldGroupPosition = grp.RootPart.GroupPosition; - grp.RootPart.GroupPosition = pos; + // NOT here // If we fail to cross the border, then reset the position of the scene object on that border. uint x = 0, y = 0; Utils.LongToUInts(newRegionHandle, out x, out y); GridRegion destination = scene.GridService.GetRegionByPosition(scene.RegionInfo.ScopeID, (int)x, (int)y); - if (destination == null || !CrossPrimGroupIntoNewRegion(destination, grp, silent)) + Vector3 oldGroupPosition = grp.RootPart.GroupPosition; + + if (destination != null) { - m_log.InfoFormat("[ENTITY TRANSFER MODULE] cross region transfer failed for object {0}",grp.UUID); + grp.RootPart.GroupPosition = pos; // only change this if we think there is anywhere to go + if (CrossPrimGroupIntoNewRegion(destination, grp, silent)) + return; // we did it + } - // Need to turn off the physics flags, otherwise the object will continue to attempt to - // move out of the region creating an infinite loop of failed attempts to cross - grp.UpdatePrimFlags(grp.RootPart.LocalId,false,grp.IsTemporary,grp.IsPhantom,false); + // no one or failed lets go back and tell physics to go on + oldGroupPosition.X = Util.Clamp(oldGroupPosition.X, 0.5f, (float)Constants.RegionSize - 0.5f); + oldGroupPosition.Y = Util.Clamp(oldGroupPosition.Y, 0.5f, (float)Constants.RegionSize - 0.5f); + oldGroupPosition.Z = Util.Clamp(oldGroupPosition.Z, 0.5f, 4096.0f); - // We are going to move the object back to the old position so long as the old position - // is in the region - oldGroupPosition.X = Util.Clamp(oldGroupPosition.X,1.0f,(float)Constants.RegionSize-1); - oldGroupPosition.Y = Util.Clamp(oldGroupPosition.Y,1.0f,(float)Constants.RegionSize-1); - oldGroupPosition.Z = Util.Clamp(oldGroupPosition.Z,1.0f,4096.0f); + grp.AbsolutePosition = oldGroupPosition; + grp.Velocity = Vector3.Zero; - grp.AbsolutePosition = oldGroupPosition; + if (grp.RootPart.PhysActor != null) + grp.RootPart.PhysActor.CrossingFailure(); - grp.ScheduleGroupForFullUpdate(); - } + grp.ScheduleGroupForFullUpdate(); } + /// /// Move the given scene object into a new region /// diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 644b78a..77e67ba 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -461,8 +461,11 @@ namespace OpenSim.Region.Framework.Scenes if (Scene != null) { - if ((Scene.TestBorderCross(val - Vector3.UnitX, Cardinals.E) || Scene.TestBorderCross(val + Vector3.UnitX, Cardinals.W) - || Scene.TestBorderCross(val - Vector3.UnitY, Cardinals.N) || Scene.TestBorderCross(val + Vector3.UnitY, Cardinals.S)) + // if ((Scene.TestBorderCross(val - Vector3.UnitX, Cardinals.E) || Scene.TestBorderCross(val + Vector3.UnitX, Cardinals.W) + // || Scene.TestBorderCross(val - Vector3.UnitY, Cardinals.N) || Scene.TestBorderCross(val + Vector3.UnitY, Cardinals.S)) + // && !IsAttachmentCheckFull() && (!Scene.LoadingPrims)) + if ((Scene.TestBorderCross(val, Cardinals.E) || Scene.TestBorderCross(val, Cardinals.W) + || Scene.TestBorderCross(val, Cardinals.N) || Scene.TestBorderCross(val, Cardinals.S)) && !IsAttachmentCheckFull() && (!Scene.LoadingPrims)) { IEntityTransferModule entityTransfer = m_scene.RequestModuleInterface(); @@ -489,7 +492,7 @@ namespace OpenSim.Region.Framework.Scenes canCross = false; break; } - + m_log.DebugFormat("[SCENE OBJECT]: Avatar {0} needs to be crossed to {1}", av.Name, destination.RegionName); } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 577c0d3..19dedde 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -2620,9 +2620,9 @@ namespace OpenSim.Region.Framework.Scenes Vector3 newpos = new Vector3(PhysActor.Position.GetBytes(), 0); if (ParentGroup.Scene.TestBorderCross(newpos, Cardinals.N) - | ParentGroup.Scene.TestBorderCross(newpos, Cardinals.S) - | ParentGroup.Scene.TestBorderCross(newpos, Cardinals.E) - | ParentGroup.Scene.TestBorderCross(newpos, Cardinals.W)) + || ParentGroup.Scene.TestBorderCross(newpos, Cardinals.S) + || ParentGroup.Scene.TestBorderCross(newpos, Cardinals.E) + || ParentGroup.Scene.TestBorderCross(newpos, Cardinals.W)) { ParentGroup.AbsolutePosition = newpos; return; diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 42e22ff..95a9741 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -732,13 +732,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { - m_crossingfailures++; - if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { base.RaiseOutOfBounds(_position); return; } - else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } @@ -3042,10 +3042,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - base.RequestPhysicsterseUpdate(); //Console.WriteLine("Move {0} at {1}", m_primName, l_position); - + /* // Check if outside region // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) @@ -3112,7 +3111,52 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; // Dont process any other motion? } // end various methods } // end outside region horizontally - + */ + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X + || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y + ) + { + // we are outside current region + // clip position to a stop just outside region and stop it only internally + // do it only once using m_crossingfailures as control + _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f); + _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + + if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + return; + } + + if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + { + // main simulator had a crossing failure + // park it inside region + _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(l_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + return; + } + + base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) { -- cgit v1.1 From b59275355e9dd8fbc126faf5bded8e61e84df4e8 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 19:44:54 +0000 Subject: don't freeze with a sitted avatar in a border without other sim. Still messy :( --- OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs | 9 ++++++++- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4 ++-- 2 files changed, 10 insertions(+), 3 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 77e67ba..53edcd6 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -537,8 +537,15 @@ namespace OpenSim.Region.Framework.Scenes return; } } + else if (RootPart.PhysActor != null) + { + RootPart.PhysActor.CrossingFailure(); + } - val = AbsolutePosition; + Vector3 oldp = AbsolutePosition; + val.X = Util.Clamp(oldp.X, 0.5f, (float)Constants.RegionSize - 0.5f); + val.Y = Util.Clamp(oldp.Y, 0.5f, (float)Constants.RegionSize - 0.5f); + val.Z = Util.Clamp(oldp.Z, 0.5f, 4096.0f); } } diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 95a9741..073241f 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -3125,7 +3125,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - if (Interlocked.Exchange(ref m_crossingfailures, 0) == 0) + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) { // tell base code only once Interlocked.Increment(ref m_crossingfailures); base.RequestPhysicsterseUpdate(); @@ -3133,7 +3133,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); return; } - if (Interlocked.Exchange(ref m_crossingfailures, 0) > 1) + if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure // park it inside region -- cgit v1.1 From f6c35cf26f759407268a46689d94e246a82b4286 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Wed, 15 Feb 2012 20:33:31 +0000 Subject: Now if chode prim.cs detects out of bounds it requests a update and blocks movement and colisions. Base code must do a PhysActor.CrossingFailure() to make it move again inside sim or delete it. --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 45 +++++++++++++++++++------ OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 20 +++++------ 2 files changed, 45 insertions(+), 20 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 073241f..a68dcb7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -166,7 +166,7 @@ namespace OpenSim.Region.Physics.OdePlugin public int m_roundsUnderMotionThreshold; private int m_crossingfailures; - public bool outofBounds; + public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; public bool _zeroFlag; // if body has been stopped @@ -732,6 +732,27 @@ namespace OpenSim.Region.Physics.OdePlugin public override void CrossingFailure() { + if (m_outofBounds) + { + _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); + _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); + _position.Z = Util.Clip(_position.Z, -100f, 50000f); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); + + m_lastposition = _position; + + _velocity = Vector3.Zero; + m_lastVelocity = _velocity; + + + if (m_type != Vehicle.TYPE_NONE) + Halt(); + + d.BodySetLinearVel(Body, 0, 0, 0); + base.RequestPhysicsterseUpdate(); + m_outofBounds = false; + } +/* int tmp = Interlocked.Increment(ref m_crossingfailures); if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) { @@ -742,6 +763,7 @@ namespace OpenSim.Region.Physics.OdePlugin { m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); } + */ } public override float Buoyancy @@ -3011,7 +3033,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if(revcount > 0) revcount--; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // Only move root prims. + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { // Old public void UpdatePositionAndVelocity(), more accuratley calculated here bool lastZeroFlag = _zeroFlag; // was it stopped @@ -3124,15 +3146,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } + /* + if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) + { // tell base code only once + Interlocked.Increment(ref m_crossingfailures); + base.RequestPhysicsterseUpdate(); + } + */ + m_outofBounds = true; + base.RequestPhysicsterseUpdate(); return; } - +/* if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) { // main simulator had a crossing failure @@ -3155,7 +3180,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); base.RequestPhysicsterseUpdate(); return; } - +*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 353db44..cf7c1d7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1536,7 +1536,7 @@ namespace OpenSim.Region.Physics.OdePlugin List removeprims = null; foreach (OdePrim chr in _activeprims) { - if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) + if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled) && !chr.m_outofBounds) { try { @@ -3410,13 +3410,13 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetTerrain(float[] heightMap, Vector3 pOffset) { - uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M) + int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra - uint heightmapHeight = regionsize + 1; + int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapHeight = regionsize + 2; - uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples - uint heightmapHeightSamples = (uint)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples + int heightmapHeightSamples = (int)regionsize + 2; // Array of height samples for ODE float[] _heightmap; @@ -3432,10 +3432,10 @@ namespace OpenSim.Region.Physics.OdePlugin float hfmax = -2000f; float minele = 0.0f; // Dont allow -ve heights - uint x = 0; - uint y = 0; - uint xx = 0; - uint yy = 0; + int x = 0; + int y = 0; + int xx = 0; + int yy = 0; // load the height samples array from the heightMap for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257 -- cgit v1.1 From 7d77ccc6593c6c3ac9f66e2e593dfb8cc719cd04 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Fri, 17 Feb 2012 21:09:00 +0000 Subject: Added simple binary serializer/deserializer to chODE. 100% untested and most like still broken --- OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 2368 +++++++++++++---------- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 28 + OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs | 167 ++ OpenSim/Region/Physics/Manager/PhysicsActor.cs | 6 + OpenSim/Region/Physics/Manager/PhysicsScene.cs | 6 + 5 files changed, 1540 insertions(+), 1035 deletions(-) create mode 100644 OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index a68dcb7..6e28bfa 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -27,6 +27,7 @@ */ using System; +using System.IO; using System.Collections.Generic; using System.Reflection; using System.Runtime.InteropServices; @@ -48,6 +49,11 @@ namespace OpenSim.Region.Physics.OdePlugin { private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + public class SerialControl + { + public object alock = new object(); + public byte[] data = new byte[0]; + } private Vector3 _position; private Vector3 _velocity; private Vector3 _torque; @@ -80,12 +86,12 @@ namespace OpenSim.Region.Physics.OdePlugin private float PID_D = 35f; private float PID_G = 25f; private bool m_usePID = false; - + private Quaternion m_APIDTarget = new Quaternion(); private float m_APIDStrength = 0.5f; private float m_APIDDamping = 0.5f; private bool m_useAPID = false; - private float m_APIDdamper = 1.0f; + private float m_APIDdamper = 1.0f; // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), // do not confuse with VEHICLE HOVER @@ -112,7 +118,7 @@ namespace OpenSim.Region.Physics.OdePlugin private bool m_taintPhysics; private bool m_collidesLand = true; private bool m_collidesWater; - public bool m_returnCollisions; + // public bool m_returnCollisions; // Default we're a Geometry private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); @@ -144,7 +150,7 @@ namespace OpenSim.Region.Physics.OdePlugin private OdeScene _parent_scene; public IntPtr m_targetSpace = IntPtr.Zero; public IntPtr prim_geom; -// public IntPtr prev_geom; + // public IntPtr prev_geom; public IntPtr _triMeshData; private IntPtr _linkJointGroup = IntPtr.Zero; @@ -163,8 +169,8 @@ namespace OpenSim.Region.Physics.OdePlugin private int throttleCounter; public int m_interpenetrationcount; public float m_collisionscore; - public int m_roundsUnderMotionThreshold; - private int m_crossingfailures; + // public int m_roundsUnderMotionThreshold; + // private int m_crossingfailures; public bool m_outofBounds; private float m_density = 10.000006836f; // Aluminum g/cm3; @@ -185,75 +191,470 @@ namespace OpenSim.Region.Physics.OdePlugin public volatile bool childPrim; internal int m_material = (int)Material.Wood; - - private int frcount = 0; // Used to limit dynamics debug output to - private int revcount = 0; // Reverse motion while > 0 private IntPtr m_body = IntPtr.Zero; - + // Vehicle properties ============================================================================================ private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings: - // HOVER_TERRAIN_ONLY - // HOVER_GLOBAL_HEIGHT - // NO_DEFLECTION_UP - // HOVER_WATER_ONLY - // HOVER_UP_ONLY - // LIMIT_MOTOR_UP - // LIMIT_ROLL_ONLY - + private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + // Linear properties private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity - //requested by LSL - private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL - private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL + //requested by LSL + private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL + private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL - - private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor - private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity - private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity - + + private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor + private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity + private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity + //Angular properties private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor -// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity + // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body - //Deflection properties + //Deflection properties // private float m_angularDeflectionEfficiency = 0; // private float m_angularDeflectionTimescale = 0; // private float m_linearDeflectionEfficiency = 0; // private float m_linearDeflectionTimescale = 0; - + //Banking properties // private float m_bankingEfficiency = 0; // private float m_bankingMix = 0; // private float m_bankingTimescale = 0; - + //Hover and Buoyancy properties private float m_VhoverHeight = 0f; -// private float m_VhoverEfficiency = 0f; + // private float m_VhoverEfficiency = 0f; private float m_VhoverTimescale = 0f; private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. - // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) - // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. - // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. - - //Attractor properties - private float m_verticalAttractionEfficiency = 1.0f; // damped + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. - - + SerialControl m_taintserial = null; + + public override byte[] Serialize(bool PhysIsRunning) + { + SerialControl sc = new SerialControl(); + + lock (sc.alock) + { + if (PhysIsRunning) + { + m_taintserial = sc; + + if (!Monitor.Wait(sc.alock, 1000)) + { + m_log.Error("[chOde] prim data serialization timed out"); + m_taintserial = null; + return new byte[0]; + } + } + else + DoSerialize(sc); + } + + return sc.data; + } + + public void DoSerialize(SerialControl sc) + { + wstreamer st = new wstreamer(); + Vector3 vtmp; + + ushort version = 2; + if (!BitConverter.IsLittleEndian) + version |= 1; + st.Wushort(version); //version lower bit codes endian type for future use + + // compact booleans in a ushort + ushort flags = 0; + + if (m_isphysical) // this should be true for now + flags |= 1; + if (m_isSelected) + flags |= 2; + if (m_isVolumeDetect) + flags |= 4; + if (m_disabled) + flags |= 8; + if (m_collidesWater) + flags |= 16; + if (m_collidesLand) + flags |= 32; + if (m_usePID) + flags |= 64; + if (m_useAPID) + flags |= 128; + if (m_useHoverPID) + flags |= 256; + if (m_throttleUpdates) + flags |= 512; + + st.Wushort(flags); + + st.Wvector3(_size); + st.Wint(m_material); + st.Wfloat(m_density); + st.Wfloat(0); // future gravity mod V3 + st.Wfloat(0); // future friction V3 + st.Wfloat(0); // future bounce V3 + +// st.Wuint((uint)m_collisionCategories); +// st.Wuint((uint)m_collisionFlags); + + if (_parent == null) + { + st.Wvector3(_position); // ?? + st.Wquat(_orientation); + } + else // for childs save offsets + { + Quaternion to; + Quaternion ipo = Quaternion.Inverse(_parent.Orientation); + + if (m_isphysical && prim_geom != IntPtr.Zero) + { + d.Vector3 dvt; + d.GeomCopyPosition(prim_geom, out dvt); + + vtmp.X = dvt.X; + vtmp.Y = dvt.Y; + vtmp.Z = dvt.Z; + + d.Quaternion dqt; + d.GeomCopyQuaternion(prim_geom, out dqt); + + to.X = dqt.X; + to.Y = dqt.Y; + to.Z = dqt.Z; + to.W = dqt.W; // rotation in world + } + else + { + vtmp = _position; + to = _orientation; + } + + vtmp -= _parent.Position; // offset in world + vtmp *= ipo; // offset in local + st.Wvector3(vtmp); + + ipo *= to; // own rotation + st.Wquat(ipo); + } + + st.Wvector3(_velocity); + st.Wvector3(m_rotationalVelocity); + st.Wvector3(_acceleration); + st.Wvector3(m_rotateEnable); + + vtmp = Vector3.Zero; + for (int i = 0; i < m_forcelist.Count; i++) + { + + vtmp += (m_forcelist[i] * 100); + } + + st.Wvector3(vtmp); // force acc + + vtmp = Vector3.Zero; + for (int i = 0; i < m_angularforcelist.Count; i++) + { + vtmp += (m_angularforcelist[i] * 100); + } + + st.Wvector3(vtmp); // angular force acc + + st.Wvector3(m_PIDTarget); + st.Wfloat(m_PIDTau); + st.Wfloat(PID_D); + st.Wfloat(PID_G); + st.Wquat(m_APIDTarget); + st.Wfloat(m_APIDStrength); + st.Wfloat(m_APIDDamping); + st.Wfloat(m_APIDdamper); + + st.Wint((int)m_PIDHoverType); + st.Wfloat(m_PIDHoverHeight); + st.Wfloat(m_PIDHoverTau); + st.Wfloat(m_targetHoverHeight); + + st.Wfloat(m_groundHeight); + st.Wfloat(m_waterHeight); + + st.Wfloat(m_buoyancy); + + // this must be last since type none ends stream + if (m_type == Vehicle.TYPE_NONE) + st.Wint((int)Vehicle.TYPE_NONE); + else + { + st.Wint((int)m_type); + + st.Wquat(Quaternion.Identity); //m_referenceFrame + + st.Wint((int)m_flags); + + st.Wvector3(m_linearMotorDirection); + st.Wfloat( + (float)Math.Sqrt(m_lLinMotorDVel.LengthSquared() / m_linearMotorDirection.LengthSquared())); + + st.Wvector3(m_linearFrictionTimescale); + st.Wfloat(m_linearMotorDecayTimescale); + st.Wfloat(m_linearMotorTimescale); + st.Wvector3(new Vector3(0, 0, 0)); //m_linearMotorOffset); + + st.Wvector3(m_angularMotorDirection); + st.Wfloat((float)Math.Sqrt(m_angularMotorDVel.LengthSquared() / m_angularMotorDirection.LengthSquared())); + + st.Wvector3(m_angularFrictionTimescale); + st.Wfloat(m_angularMotorDecayTimescale); + st.Wfloat(m_angularMotorTimescale); + + st.Wfloat(0); //m_linearDeflectionEfficiency); + st.Wfloat(1000); //m_linearDeflectionTimescale); + + st.Wfloat(0); //m_angularDeflectionEfficiency); + st.Wfloat(120); //m_angularDeflectionTimescale); + + st.Wfloat(0); // m_bankingEfficiency); + st.Wfloat(0); //m_bankingMix); + st.Wfloat(1000); //m_bankingTimescale); + st.Wfloat(m_VhoverHeight); + st.Wfloat(0.5f); //m_VhoverEfficiency); + st.Wfloat(m_VhoverTimescale); + st.Wfloat(m_VehicleBuoyancy); + + st.Wfloat(m_verticalAttractionEfficiency); + st.Wfloat(m_verticalAttractionTimescale); + } + sc.data = st.close(); + m_taintserial = null; + Monitor.PulseAll(sc.alock); + } + + public bool DeSerialize(byte[] data) + { + rstreamer st = new rstreamer(data); + + int version =st.Rushort(); //version + + // merge booleans in a ushort + ushort flags = st.Rushort(); + if ((flags & 1) != 0) + m_isphysical = true; + if ((flags & 2) != 0) + m_taintselected = true; + if ((flags & 4) != 0) + m_isVolumeDetect = true; + if ((flags & 8) != 0) + m_taintdisable = true; + if ((flags & 16) != 0) + m_taintCollidesWater = true; + if ((flags & 32) != 0) + m_collidesLand = true; + if ((flags & 64) != 0) + m_usePID = true; + if ((flags & 128) != 0) + m_useAPID = true; + if ((flags & 256) != 0) + m_useHoverPID = true; + if ((flags & 512) != 0) + m_throttleUpdates = true; + + _size = st.Rvector3(); + m_taintsize = _size; + + m_material= st.Rint(); + m_density = st.Rfloat(); + st.Rfloat(); // future gravity mod V3 + st.Rfloat(); // future friction V3 + st.Rfloat(); // future bounce V3 + +// m_collisionCategories = (CollisionCategories)st.Ruint(); +// m_collisionFlags = (CollisionCategories) st.Ruint(); + + if (m_taintparent == null) + { + st.Rvector3(); // ignore old position sop/sog as to tell the new one + m_taintrot = st.Rquat(); // + _orientation = m_taintrot; + } + else + { + m_taintrot = _parent.Orientation; + m_taintposition = st.Rvector3(); // ?? + _position = m_taintposition; + + m_taintposition *= m_taintrot; + m_taintposition += _parent.Position; + + m_taintrot *= st.Rquat(); // + _orientation = m_taintrot; + } + + m_taintVelocity = st.Rvector3(); + m_rotationalVelocity = st.Rvector3(); + + _acceleration = st.Rvector3(); + m_rotateEnableRequest = st.Rvector3(); + m_rotateEnableUpdate = true; + + Vector3 vtmp; + + vtmp = st.Rvector3(); // forces acc + m_forcelist.Add(vtmp); + m_taintforce = true; + + vtmp = st.Rvector3(); // angular forces acc + m_angularforcelist.Add(vtmp); + m_taintaddangularforce = true; + + m_PIDTarget = st.Rvector3(); + m_PIDTau = st.Rfloat(); + PID_D = st.Rfloat(); + PID_G = st.Rfloat(); + + m_APIDTarget = st.Rquat(); + m_APIDStrength = st.Rfloat(); + m_APIDDamping = st.Rfloat(); + m_APIDdamper = st.Rfloat(); + + m_PIDHoverType = (PIDHoverType) st.Rint(); + m_PIDHoverHeight = st.Rfloat(); + m_PIDHoverTau = st.Rfloat(); + m_targetHoverHeight = st.Rfloat(); + + m_groundHeight = st.Rfloat(); + m_waterHeight = st.Rfloat(); + + m_buoyancy = st.Rfloat(); + + + // this must be last since type none ends stream + + m_type = (Vehicle) st.Rint(); + + if (m_type != Vehicle.TYPE_NONE) + { + float ftmp; + + st.Rquat(); //m_referenceFrame + + m_flags = (VehicleFlag) st.Rint(); + + m_linearMotorDirection = st.Rvector3(); + + ftmp = st.Rfloat(); + m_lLinMotorDVel = m_linearMotorDirection * ftmp; + + m_linearFrictionTimescale = st.Rvector3(); + m_linearMotorDecayTimescale = st.Rfloat(); + m_linearMotorTimescale = st.Rfloat(); + st.Rvector3(); //m_linearMotorOffset); + + m_angularMotorDirection = st.Rvector3(); + ftmp = st.Rfloat(); + m_angularMotorDVel = m_angularMotorDirection * ftmp; + + m_angularFrictionTimescale = st.Rvector3(); + m_angularMotorDecayTimescale = st.Rfloat(); + m_angularMotorTimescale = st.Rfloat(); + + st.Rfloat(); //m_linearDeflectionEfficiency); + st.Rfloat(); //m_linearDeflectionTimescale); + + st.Rfloat(); //m_angularDeflectionEfficiency); + st.Rfloat(); //m_angularDeflectionTimescale); + + st.Rfloat(); // m_bankingEfficiency); + st.Rfloat(); //m_bankingMix); + st.Rfloat(); //m_bankingTimescale); + + m_VhoverHeight = st.Rfloat(); + st.Rfloat(); //m_VhoverEfficiency); + m_VhoverTimescale = st.Rfloat(); + + m_VehicleBuoyancy = st.Rfloat(); + + m_verticalAttractionEfficiency = st.Rfloat(); + m_verticalAttractionTimescale = st.Rfloat(); + } + st.close(); + return true; + } + + public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, PhysicsActor parent, + PrimitiveBaseShape pbs, CollisionLocker dode, uint localid, byte[] sdata) + { + m_localID = localid; + ode = dode; + + if (parent == null) + { + m_taintparent = null; + + if (!pos.IsFinite()) + { + pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), + parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); + m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); + } + + _position = pos; + m_taintposition = pos; + } + else + m_taintparent = parent; + + body_autodisable_frames = parent_scene.bodyFramesAutoDisable; + + prim_geom = IntPtr.Zero; + + _mesh = null; + m_meshfailed = false; + _pbs = pbs; + + _parent_scene = parent_scene; + m_targetSpace = (IntPtr)0; + + if(sdata != null && sdata.Length > 1) + DeSerialize(sdata); + + if (m_isphysical) + m_targetSpace = _parent_scene.space; + + m_primName = primName; + m_taintserial = null; + m_taintadd = true; + _parent_scene.AddPhysicsActorTaint(this); + // don't do .add() here; old geoms get recycled with the same hash + } public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode, uint localid) @@ -266,7 +667,7 @@ namespace OpenSim.Region.Physics.OdePlugin parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); } - + _position = pos; m_taintposition = pos; PID_D = parent_scene.bodyPIDD; @@ -275,9 +676,8 @@ namespace OpenSim.Region.Physics.OdePlugin // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; body_autodisable_frames = parent_scene.bodyFramesAutoDisable; - prim_geom = IntPtr.Zero; -// prev_geom = IntPtr.Zero; + // prev_geom = IntPtr.Zero; if (!pos.IsFinite()) { @@ -306,7 +706,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene = parent_scene; m_targetSpace = (IntPtr)0; -// if (pos.Z < 0) + // if (pos.Z < 0) if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) m_isphysical = false; else @@ -317,6 +717,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_isphysical) m_targetSpace = _parent_scene.space; } + + m_taintserial = null; m_primName = primName; m_taintadd = true; _parent_scene.AddPhysicsActorTaint(this); @@ -325,7 +727,7 @@ namespace OpenSim.Region.Physics.OdePlugin public override int PhysicsActorType { - get { return (int) ActorTypes.Prim; } + get { return (int)ActorTypes.Prim; } set { return; } } @@ -337,9 +739,11 @@ namespace OpenSim.Region.Physics.OdePlugin public override uint LocalID { - set { + set + { //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); - m_localID = value; } + m_localID = value; + } } public override bool Grabbed @@ -349,9 +753,10 @@ namespace OpenSim.Region.Physics.OdePlugin public override bool Selected { - set { - -//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); + set + { + + //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); // This only makes the object not collidable if the object // is physical or the object is modified somehow *IN THE FUTURE* // without this, if an avatar selects prim, they can walk right @@ -367,21 +772,21 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } - if(m_isSelected) disableBodySoft(); + if (m_isSelected) disableBodySoft(); } } public override bool IsPhysical { get { return m_isphysical; } - set - { - m_isphysical = value; - if (!m_isphysical) - { // Zero the remembered last velocity - m_lastVelocity = Vector3.Zero; - if (m_type != Vehicle.TYPE_NONE) Halt(); - } + set + { + m_isphysical = value; + if (!m_isphysical) + { // Zero the remembered last velocity + m_lastVelocity = Vector3.Zero; + if (m_type != Vehicle.TYPE_NONE) Halt(); + } } } @@ -430,7 +835,9 @@ namespace OpenSim.Region.Physics.OdePlugin { get { return _position; } - set { _position = value; + set + { + _position = value; //m_log.Info("[PHYSICS]: " + _position.ToString()); } } @@ -481,29 +888,29 @@ namespace OpenSim.Region.Physics.OdePlugin public override void VehicleFloatParam(int param, float value) { - ProcessFloatVehicleParam((Vehicle) param, value); + ProcessFloatVehicleParam((Vehicle)param, value); } public override void VehicleVectorParam(int param, Vector3 value) { - ProcessVectorVehicleParam((Vehicle) param, value); + ProcessVectorVehicleParam((Vehicle)param, value); } public override void VehicleRotationParam(int param, Quaternion rotation) { - ProcessRotationVehicleParam((Vehicle) param, rotation); + ProcessRotationVehicleParam((Vehicle)param, rotation); } public override void VehicleFlags(int param, bool remove) { ProcessVehicleFlags(param, remove); } - + public override void SetVolumeDetect(int param) { lock (_parent_scene.OdeLock) { - m_isVolumeDetect = (param!=0); + m_isVolumeDetect = (param != 0); } } @@ -536,9 +943,9 @@ namespace OpenSim.Region.Physics.OdePlugin return Vector3.Zero; Vector3 returnVelocity = Vector3.Zero; - returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2; - returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2; - returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2; + returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; + returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; + returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; return returnVelocity; } set @@ -546,8 +953,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (value.IsFinite()) { _velocity = value; - if (_velocity.ApproxEquals(Vector3.Zero,0.001f)) - _acceleration = Vector3.Zero; + if (_velocity.ApproxEquals(Vector3.Zero, 0.001f)) + _acceleration = Vector3.Zero; m_taintVelocity = value; _parent_scene.AddPhysicsActorTaint(this); @@ -604,17 +1011,18 @@ namespace OpenSim.Region.Physics.OdePlugin if (QuaternionIsFinite(value)) { _orientation = value; - } + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); } } - - - public override bool FloatOnWater + + + public override bool FloatOnWater { - set { + set + { m_taintCollidesWater = value; _parent_scene.AddPhysicsActorTaint(this); } @@ -624,8 +1032,8 @@ namespace OpenSim.Region.Physics.OdePlugin { } - public override Vector3 PIDTarget - { + public override Vector3 PIDTarget + { set { if (value.IsFinite()) @@ -634,16 +1042,16 @@ namespace OpenSim.Region.Physics.OdePlugin } else m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); - } + } } public override bool PIDActive { set { m_usePID = value; } } public override float PIDTau { set { m_PIDTau = value; } } - - // For RotLookAt - public override Quaternion APIDTarget { set { m_APIDTarget = value; } } - public override bool APIDActive { set { m_useAPID = value; } } - public override float APIDStrength { set { m_APIDStrength = value; } } - public override float APIDDamping { set { m_APIDDamping = value; } } + + // For RotLookAt + public override Quaternion APIDTarget { set { m_APIDTarget = value; } } + public override bool APIDActive { set { m_useAPID = value; } } + public override float APIDStrength { set { m_APIDStrength = value; } } + public override float APIDDamping { set { m_APIDDamping = value; } } public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } public override bool PIDHoverActive { set { m_useHoverPID = value; } } @@ -665,13 +1073,13 @@ namespace OpenSim.Region.Physics.OdePlugin public override Vector3 Acceleration // client updates read data via here { - get - { + get + { if (_zeroFlag) { return Vector3.Zero; } - return _acceleration; + return _acceleration; } set { _acceleration = value; } } @@ -752,18 +1160,18 @@ namespace OpenSim.Region.Physics.OdePlugin base.RequestPhysicsterseUpdate(); m_outofBounds = false; } -/* - int tmp = Interlocked.Increment(ref m_crossingfailures); - if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - base.RaiseOutOfBounds(_position); - return; - } - else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) - { - m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); - } - */ + /* + int tmp = Interlocked.Increment(ref m_crossingfailures); + if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + */ } public override float Buoyancy @@ -806,26 +1214,26 @@ namespace OpenSim.Region.Physics.OdePlugin public void SetGeom(IntPtr geom) { - if(prim_geom != IntPtr.Zero) - { - // Remove any old entries -//string tPA; -//_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); -//Console.WriteLine("**** Remove {0}", tPA); - if(_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); - if(_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); + if (prim_geom != IntPtr.Zero) + { + // Remove any old entries + //string tPA; + //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); + //Console.WriteLine("**** Remove {0}", tPA); + if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); + if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); d.GeomDestroy(prim_geom); - } + } prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); + //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { _parent_scene.geom_name_map[prim_geom] = this.m_primName; _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); -//Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); + //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); } if (childPrim) @@ -833,7 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null && _parent is OdePrim) { OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); + //Console.WriteLine("SetGeom calls ChildSetGeom"); parent.ChildSetGeom(this); } } @@ -848,7 +1256,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyEnable(Body); if (m_type != Vehicle.TYPE_NONE) - Enable(Body, _parent_scene); + Enable(Body, _parent_scene); } m_disabled = false; @@ -892,9 +1300,9 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableFlag(Body, true); d.BodySetAutoDisableSteps(Body, body_autodisable_frames); - + // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + d.BodySetGravityMode(Body, false); m_interpenetrationcount = 0; m_collisionscore = 0; @@ -918,19 +1326,19 @@ namespace OpenSim.Region.Physics.OdePlugin float returnMass = 0; float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; - float hollowVolume = hollowAmount * hollowAmount; - + float hollowVolume = hollowAmount * hollowAmount; + switch (_pbs.ProfileShape) { case ProfileShape.Square: // default box if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; @@ -948,31 +1356,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { //a tube volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); - tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); - volume -= volume*tmp*tmp; - + tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume * tmp * tmp; + if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; - + switch (_pbs.HollowShape) - { + { case HollowShape.Square: case HollowShape.Same: break; case HollowShape.Circle: - hollowVolume *= 0.78539816339f;; + hollowVolume *= 0.78539816339f; ; break; case HollowShape.Triangle: @@ -981,23 +1389,23 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.Circle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.78539816339f; // elipse base if (hollowAmount > 0.0) - { + { switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1013,25 +1421,25 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); - + if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Circle: break; @@ -1047,31 +1455,31 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } break; case ProfileShape.HalfCircle: if (_pbs.PathCurve == (byte)Extrusion.Curve1) { - volume *= 0.52359877559829887307710723054658f; + volume *= 0.52359877559829887307710723054658f; } break; case ProfileShape.EquilateralTriangle: if (_pbs.PathCurve == (byte)Extrusion.Straight) - { + { volume *= 0.32475953f; if (hollowAmount > 0.0) - { + { // calculate the hollow volume by it's shape compared to the prim shape switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1091,24 +1499,24 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } + } else if (_pbs.PathCurve == (byte)Extrusion.Curve1) - { + { volume *= 0.32475953f; volume *= 0.01f * (float)(200 - _pbs.PathScaleX); tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); volume *= (1.0f - tmp * tmp); if (hollowAmount > 0.0) - { + { hollowVolume *= hollowAmount; switch (_pbs.HollowShape) - { + { case HollowShape.Same: case HollowShape.Triangle: hollowVolume *= .25f; @@ -1126,15 +1534,15 @@ namespace OpenSim.Region.Physics.OdePlugin default: hollowVolume = 0; break; - } - volume *= (1.0f - hollowVolume); } + volume *= (1.0f - hollowVolume); } - break; + } + break; default: break; - } + } @@ -1148,7 +1556,7 @@ namespace OpenSim.Region.Physics.OdePlugin float profileEnd; if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) - { + { taperX1 = _pbs.PathScaleX * 0.01f; if (taperX1 > 1.0f) taperX1 = 2.0f - taperX1; @@ -1158,9 +1566,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (taperY1 > 1.0f) taperY1 = 2.0f - taperY1; taperY = 1.0f - taperY1; - } + } else - { + { taperX = _pbs.PathTaperX * 0.01f; if (taperX < 0.0f) taperX = -taperX; @@ -1168,10 +1576,10 @@ namespace OpenSim.Region.Physics.OdePlugin taperY = _pbs.PathTaperY * 0.01f; if (taperY < 0.0f) - taperY = -taperY; + taperY = -taperY; taperY1 = 1.0f - taperY; - } + } volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); @@ -1180,7 +1588,7 @@ namespace OpenSim.Region.Physics.OdePlugin pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; volume *= (pathEnd - pathBegin); -// this is crude aproximation + // this is crude aproximation profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; volume *= (profileEnd - profileBegin); @@ -1189,8 +1597,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. -// else if (returnMass > _parent_scene.maximumMassObject) -// returnMass = _parent_scene.maximumMassObject; + // else if (returnMass > _parent_scene.maximumMassObject) + // returnMass = _parent_scene.maximumMassObject; @@ -1230,7 +1638,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void setMass() { - if (Body != (IntPtr) 0) + if (Body != (IntPtr)0) { float newmass = CalculateMass(); @@ -1260,7 +1668,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + d.BodyDestroy(Body); lock (childrenPrim) { @@ -1279,7 +1687,7 @@ namespace OpenSim.Region.Physics.OdePlugin else { _parent_scene.remActivePrim(this); - + m_collisionCategories &= ~CollisionCategories.Body; m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); @@ -1289,7 +1697,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - + Body = IntPtr.Zero; } } @@ -1345,10 +1753,10 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { - // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer - // { - SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); - // } + // if (prim_geom == IntPtr.Zero) // setGeom takes care of phys engine recreate and prim_geom pointer + // { + SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); + // } } catch (AccessViolationException) { @@ -1357,14 +1765,14 @@ namespace OpenSim.Region.Physics.OdePlugin } - // if (IsPhysical && Body == (IntPtr) 0) - // { - // Recreate the body - // m_interpenetrationcount = 0; - // m_collisionscore = 0; + // if (IsPhysical && Body == (IntPtr) 0) + // { + // Recreate the body + // m_interpenetrationcount = 0; + // m_collisionscore = 0; - // enableBody(); - // } + // enableBody(); + // } } public void ProcessTaints(float timestep) //============================================================================= @@ -1373,37 +1781,37 @@ namespace OpenSim.Region.Physics.OdePlugin { changeadd(timestep); } - + if (prim_geom != IntPtr.Zero) { - if (!_position.ApproxEquals(m_taintposition, 0f)) + if (!_position.ApproxEquals(m_taintposition, 0f)) { - changemove(timestep); + changemove(timestep); } - if (m_taintrot != _orientation) - { - if(childPrim && IsPhysical) // For physical child prim... - { - rotate(timestep); - // KF: ODE will also rotate the parent prim! - // so rotate the root back to where it was - OdePrim parent = (OdePrim)_parent; - parent.rotate(timestep); + if (m_taintrot != _orientation) + { + if (childPrim && IsPhysical) // For physical child prim... + { + rotate(timestep); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(timestep); } else { - //Just rotate the prim - rotate(timestep); - } + //Just rotate the prim + rotate(timestep); + } } // - + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) { changePhysicsStatus(timestep); }// - if (!_size.ApproxEquals(m_taintsize,0f)) + if (!_size.ApproxEquals(m_taintsize, 0f)) changesize(timestep); // @@ -1434,10 +1842,14 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); -/* obsolete - if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) - changeAngularLock(timestep); - */ + + if (m_taintserial != null) + DoSerialize(m_taintserial); + + /* obsolete + if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) + changeAngularLock(timestep); + */ } else { @@ -1445,16 +1857,16 @@ namespace OpenSim.Region.Physics.OdePlugin } } -/* obsolete - private void changeAngularLock(float timestep) - { - if (_parent == null) - { - m_angularLock = m_taintAngularLock; - m_angularLockSet = true; - } - } - */ + /* obsolete + private void changeAngularLock(float timestep) + { + if (_parent == null) + { + m_angularLock = m_taintAngularLock; + m_angularLockSet = true; + } + } + */ private void changelink(float timestep) { // If the newly set parent is not null @@ -1489,7 +1901,7 @@ namespace OpenSim.Region.Physics.OdePlugin childPrim = false; //_parent = null; } - + /* if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) d.JointGroupDestroy(_linkJointGroup); @@ -1498,7 +1910,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_linkJoint = (IntPtr)0; */ } - + _parent = m_taintparent; m_taintPhysics = m_isphysical; } @@ -1512,8 +1924,8 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body == IntPtr.Zero) { Body = d.BodyCreate(_parent_scene.world); - // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + // disconnect from world gravity so we can apply buoyancy + d.BodySetGravityMode(Body, false); setMass(); } @@ -1524,7 +1936,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (!childrenPrim.Contains(prim)) { childrenPrim.Add(prim); - + foreach (OdePrim prm in childrenPrim) { d.Mass m2; @@ -1546,7 +1958,7 @@ namespace OpenSim.Region.Physics.OdePlugin } foreach (OdePrim prm in childrenPrim) { - + prm.m_collisionCategories |= CollisionCategories.Body; prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); @@ -1571,7 +1983,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.GeomSetBody(prm.prim_geom, Body); prm.childPrim = true; - d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); + d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z); //d.GeomSetOffsetPosition(prim.prim_geom, // (Position.X - prm.Position.X) - pMass.c.X, // (Position.Y - prm.Position.Y) - pMass.c.Y, @@ -1668,7 +2080,7 @@ namespace OpenSim.Region.Physics.OdePlugin ParentPrim(prm); } } - + } private void ChildDelink(OdePrim odePrim) @@ -1719,7 +2131,7 @@ namespace OpenSim.Region.Physics.OdePlugin // in between the disabling and the collision properties setting // which would wake the physical body up from a soft disabling and potentially cause it to fall // through the ground. - + // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select // just one part of the assembly, the rest of the assembly is non-selected and still simulating, // so that causes the selected part to wake up and continue moving. @@ -1752,12 +2164,12 @@ namespace OpenSim.Region.Physics.OdePlugin { disableBodySoft(); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } } @@ -1780,12 +2192,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); } - if (Body != IntPtr.Zero) + if (Body != IntPtr.Zero) { d.BodySetLinearVel(Body, 0f, 0f, 0f); d.BodySetForce(Body, 0f, 0f, 0f); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - d.BodySetTorque (Body, 0.0f, 0.0f, 0.0f); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); } if (m_isphysical) @@ -1913,7 +2325,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_meshfailed = true; } // createmesh returns null when it's a shape that isn't a cube. - // m_log.Debug(m_localID); + // m_log.Debug(m_localID); } } @@ -1948,7 +2360,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (m_isphysical) { -// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! + // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! if (!m_taintremove && !childPrim) { if (Body == IntPtr.Zero) @@ -1972,8 +2384,8 @@ namespace OpenSim.Region.Physics.OdePlugin OdePrim odParent = (OdePrim)_parent; if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) { -// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? -Console.WriteLine("ODEPrim JointCreateFixed !!!"); + // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? + Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1991,8 +2403,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } //else - // { - //m_log.Debug("[BUG]: race!"); + // { + //m_log.Debug("[BUG]: race!"); //} } else @@ -2031,15 +2443,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); myrot.W = _orientation.W; if (Body != IntPtr.Zero) { - // KF: If this is a root prim do BodySet + // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - } - else - { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); } - + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); + } + resetCollisionAccounting(); m_taintrot = _orientation; } @@ -2111,7 +2523,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void changesize(float timestamp) { - + string oldname = _parent_scene.geom_name_map[prim_geom]; if (_size.X <= 0) _size.X = 0.01f; @@ -2170,7 +2582,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); CreateGeom(m_targetSpace, mesh); - + } else { @@ -2210,7 +2622,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_taintsize = _size; } - + public void changefloatonwater(float timestep) { @@ -2398,7 +2810,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } d.BodyEnable(Body); d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); - + } m_angularforcelist.Clear(); } @@ -2420,7 +2832,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (Body != IntPtr.Zero) d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); } - + //resetCollisionAccounting(); } m_taintVelocity = Vector3.Zero; @@ -2428,9 +2840,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public void UpdatePositionAndVelocity() { - return; // moved to the Move () method + return; // moved to the Move () method } - + public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) { obj.I.M00 = pMat[0, 0]; @@ -2455,7 +2867,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { _parent_scene.remCollisionEventReporting(this); m_eventsubscription = 0; - } + } public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) { @@ -2499,9 +2911,9 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); public static Matrix4 Adjoint(Matrix4 pMat) { Matrix4 adjointMatrix = new Matrix4(); - for (int i=0; i<4; i++) + for (int i = 0; i < 4; i++) { - for (int j=0; j<4; j++) + for (int j = 0; j < 4; j++) { Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); } @@ -2524,7 +2936,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { if (j == iCol) continue; - Matrix4SetValue(ref minor, m,n, matrix[i, j]); + Matrix4SetValue(ref minor, m, n, matrix[i, j]); n++; } m++; @@ -2622,18 +3034,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); private static float determinant3x3(Matrix4 pMat) { float det = 0; - float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; - float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; - float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; - float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; - float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; - float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; + float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2]; + float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0]; + float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1]; + float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2]; + float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0]; + float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1]; det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); return det; } - + private static void DMassCopy(ref d.Mass src, ref d.Mass dst) { dst.c.W = src.c.W; @@ -2690,15 +3102,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; m_VehicleBuoyancy = pValue; break; -// case Vehicle.HOVER_EFFICIENCY: -// if (pValue < 0f) pValue = 0f; -// if (pValue > 1f) pValue = 1f; -// m_VhoverEfficiency = pValue; -// break; + // case Vehicle.HOVER_EFFICIENCY: + // if (pValue < 0f) pValue = 0f; + // if (pValue > 1f) pValue = 1f; + // m_VhoverEfficiency = pValue; + // break; case Vehicle.HOVER_HEIGHT: m_VhoverHeight = pValue; break; @@ -2731,12 +3143,12 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); if (pValue < 0.1f) pValue = 0.1f; m_verticalAttractionTimescale = pValue; break; - + // These are vector properties but the engine lets you use a single float value to // set all of the components to the same value case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue > 30f) pValue = 30f; - if (pValue < 0.1f) pValue = 0.1f; + if (pValue > 30f) pValue = 30f; + if (pValue < 0.1f) pValue = 0.1f; m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: @@ -2744,7 +3156,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < 0.1f) pValue = 0.1f; + if (pValue < 0.1f) pValue = 0.1f; m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2756,7 +3168,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; } - + }//end ProcessFloatVehicleParam internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) @@ -2764,29 +3176,29 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); switch (pParam) { case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X > 30f) pValue.X = 30f; - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y > 30f) pValue.Y = 30f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z > 30f) pValue.Z = 30f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X > 30f) pValue.X = 30f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y > 30f) pValue.Y = 30f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z > 30f) pValue.Z = 30f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // Limit requested angular speed to 2 rps= 4 pi rads/sec - if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; - if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; - if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; - if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; - if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; - if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f; + if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f; + if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f; UpdateAngDecay(); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < 0.1f) pValue.X = 0.1f; - if (pValue.Y < 0.1f) pValue.Y = 0.1f; - if (pValue.Z < 0.1f) pValue.Z = 0.1f; + if (pValue.X < 0.1f) pValue.X = 0.1f; + if (pValue.Y < 0.1f) pValue.Y = 0.1f; + if (pValue.Z < 0.1f) pValue.Z = 0.1f; m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_MOTOR_DIRECTION: @@ -2797,7 +3209,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); break; } - + }//end ProcessVectorVehicleParam internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) @@ -2808,31 +3220,31 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_referenceFrame = pValue; break; } - + }//end ProcessRotationVehicleParam - + internal void ProcessVehicleFlags(int pParam, bool remove) { if (remove) { - m_flags &= ~((VehicleFlag)pParam); - } - else - { - m_flags |= (VehicleFlag)pParam; - } + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } } - + internal void ProcessTypeChange(Vehicle pType) { - // Set Defaults For Type + // Set Defaults For Type m_type = pType; switch (pType) { case Vehicle.TYPE_SLED: m_linearFrictionTimescale = new Vector3(30, 1, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1000; m_linearMotorDecayTimescale = 120; m_angularMotorDirection = Vector3.Zero; @@ -2840,7 +3252,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1000; m_angularMotorDecayTimescale = 120; m_VhoverHeight = 0; -// m_VhoverEfficiency = 1; + // m_VhoverEfficiency = 1; m_VhoverTimescale = 10; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 1; @@ -2859,7 +3271,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); case Vehicle.TYPE_CAR: m_linearFrictionTimescale = new Vector3(100, 2, 1000); m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 1; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2867,7 +3279,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 1; m_angularMotorDecayTimescale = 0.8f; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0; + // m_VhoverEfficiency = 0; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // // m_linearDeflectionEfficiency = 1; @@ -2886,8 +3298,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); break; case Vehicle.TYPE_BOAT: m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10,10,10); -// m_lLinMotorVel = Vector3.Zero; + m_angularFrictionTimescale = new Vector3(10, 10, 10); + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 5; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2895,7 +3307,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 2; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0.5f; @@ -2908,15 +3320,15 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); // m_bankingMix = 0.8f; // m_bankingTimescale = 1; // m_referenceFrame = Quaternion.Identity; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.LIMIT_MOTOR_UP); break; case Vehicle.TYPE_AIRPLANE: m_linearFrictionTimescale = new Vector3(200, 10, 5); m_angularFrictionTimescale = new Vector3(20, 20, 20); -// m_lLinMotorVel = Vector3.Zero; + // m_lLinMotorVel = Vector3.Zero; m_linearMotorTimescale = 2; m_linearMotorDecayTimescale = 60; m_angularMotorDirection = Vector3.Zero; @@ -2924,7 +3336,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 4; m_angularMotorDecayTimescale = 4; m_VhoverHeight = 0; -// m_VhoverEfficiency = 0.5f; + // m_VhoverEfficiency = 0.5f; m_VhoverTimescale = 1000; m_VehicleBuoyancy = 0; // m_linearDeflectionEfficiency = 0.5f; @@ -2951,7 +3363,7 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); m_angularMotorTimescale = 6; m_angularMotorDecayTimescale = 10; m_VhoverHeight = 5; -// m_VhoverEfficiency = 0.8f; + // m_VhoverEfficiency = 0.8f; m_VhoverTimescale = 10; m_VehicleBuoyancy = 1; // m_linearDeflectionEfficiency = 0; @@ -2981,63 +3393,48 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } - internal void Halt() - { // Kill all motions, when non-physical - // m_linearMotorDirection = Vector3.Zero; - m_lLinMotorDVel = Vector3.Zero; - m_lLinObjectVel = Vector3.Zero; - m_wLinObjectVel = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; - m_lastAngularVelocity = Vector3.Zero; - m_angularMotorDVel = Vector3.Zero; - _acceleration = Vector3.Zero; - } - - private void UpdateLinDecay() - { -// if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X; -// if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y; -// if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - m_lLinMotorDVel.X = m_linearMotorDirection.X; - m_lLinMotorDVel.Y = m_linearMotorDirection.Y; - m_lLinMotorDVel.Z = m_linearMotorDirection.Z; - } // else let the motor decay on its own - - private void UpdateAngDecay() - { -// if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X; -// if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y; -// if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z; - m_angularMotorDVel.X = m_angularMotorDirection.X; - m_angularMotorDVel.Y = m_angularMotorDirection.Y; - m_angularMotorDVel.Z = m_angularMotorDirection.Z; - } // else let the motor decay on its own - + internal void Halt() + { // Kill all motions, when non-physical + // m_linearMotorDirection = Vector3.Zero; + m_lLinMotorDVel = Vector3.Zero; + m_lLinObjectVel = Vector3.Zero; + m_wLinObjectVel = Vector3.Zero; + m_angularMotorDirection = Vector3.Zero; + m_lastAngularVelocity = Vector3.Zero; + m_angularMotorDVel = Vector3.Zero; + _acceleration = Vector3.Zero; + } + + private void UpdateLinDecay() + { + m_lLinMotorDVel.X = m_linearMotorDirection.X; + m_lLinMotorDVel.Y = m_linearMotorDirection.Y; + m_lLinMotorDVel.Z = m_linearMotorDirection.Z; + } // else let the motor decay on its own + + private void UpdateAngDecay() + { + m_angularMotorDVel.X = m_angularMotorDirection.X; + m_angularMotorDVel.Y = m_angularMotorDirection.Y; + m_angularMotorDVel.Z = m_angularMotorDirection.Z; + } // else let the motor decay on its own + public void Move(float timestep) { float fx = 0; float fy = 0; float fz = 0; Vector3 linvel; // velocity applied, including any reversal - int outside = 0; - + // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. // This is a temp patch until proper region crossing is developed. - - int failureLimit = _parent_scene.geomCrossingFailuresBeforeOutofbounds; - float fence = _parent_scene.geomRegionFence; - - frcount++; // used to limit debug comment output - if (frcount > 50) - frcount = 0; - - if(revcount > 0) revcount--; - + + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. { - // Old public void UpdatePositionAndVelocity(), more accuratley calculated here - bool lastZeroFlag = _zeroFlag; // was it stopped - + // Old public void UpdatePositionAndVelocity(), more accuratley calculated here + bool lastZeroFlag = _zeroFlag; // was it stopped + d.Vector3 vec = d.BodyGetPosition(Body); Vector3 l_position = Vector3.Zero; l_position.X = vec.X; @@ -3045,95 +3442,26 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); l_position.Z = vec.Z; m_lastposition = _position; _position = l_position; - + d.Quaternion ori = d.BodyGetQuaternion(Body); - // Quaternion l_orientation = Quaternion.Identity; + // Quaternion l_orientation = Quaternion.Identity; _orientation.X = ori.X; _orientation.Y = ori.Y; _orientation.Z = ori.Z; _orientation.W = ori.W; m_lastorientation = _orientation; - + d.Vector3 vel = d.BodyGetLinearVel(Body); m_lastVelocity = _velocity; _velocity.X = vel.X; _velocity.Y = vel.Y; _velocity.Z = vel.Z; _acceleration = ((_velocity - m_lastVelocity) / timestep); - + d.Vector3 torque = d.BodyGetTorque(Body); _torque = new Vector3(torque.X, torque.Y, torque.Z); - - -//Console.WriteLine("Move {0} at {1}", m_primName, l_position); - /* - // Check if outside region - // In Scene.cs/CrossPrimGroupIntoNewRegion the object is checked for 0.1M from border! - if (l_position.X > ((float)_parent_scene.WorldExtents.X - fence)) - { - l_position.X = ((float)_parent_scene.WorldExtents.X - fence); - outside = 1; - } - - if (l_position.X < fence) - { - l_position.X = fence; - outside = 2; - } - if (l_position.Y > ((float)_parent_scene.WorldExtents.Y - fence)) - { - l_position.Y = ((float)_parent_scene.WorldExtents.Y - fence); - outside = 3; - } - - if (l_position.Y < fence) - { - l_position.Y = fence; - outside = 4; - } - - if (outside > 0) - { - -//Console.WriteLine("Border {0} fence={1}", l_position, fence); - if (fence > 0.0f) // bounce object off boundary - { - if (revcount == 0) - { - if (outside < 3) - { - _velocity.X = -_velocity.X; - } - else - { - _velocity.Y = -_velocity.Y; - } - if (m_type != Vehicle.TYPE_NONE) Halt(); - _position = l_position; - m_taintposition = _position; - m_lastVelocity = _velocity; - _acceleration = Vector3.Zero; - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); - base.RequestPhysicsterseUpdate(); - - revcount = 25; // wait for object to move away from border - } - } // else old crossing mode - else if (m_crossingfailures < failureLimit) - { // keep trying to cross? - _position = l_position; - //_parent_scene.remActivePrim(this); - if (_parent == null) base.RequestPhysicsterseUpdate(); - return; // Dont process any other motion? - } - else - { // Too many tries - if (_parent == null) base.RaiseOutOfBounds(l_position); - return; // Dont process any other motion? - } // end various methods - } // end outside region horizontally - */ + + if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y ) @@ -3146,41 +3474,11 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); _position.Z = Util.Clip(l_position.Z, -100f, 50000f); d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); d.BodySetLinearVel(Body, 0, 0, 0); - /* - if (Interlocked.Exchange(ref m_crossingfailures, m_crossingfailures) == 0) - { // tell base code only once - Interlocked.Increment(ref m_crossingfailures); - base.RequestPhysicsterseUpdate(); - } - */ m_outofBounds = true; base.RequestPhysicsterseUpdate(); return; } -/* - if (Interlocked.Exchange(ref m_crossingfailures, 0) != 0) - { - // main simulator had a crossing failure - // park it inside region - _position.X = Util.Clip(l_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); - _position.Y = Util.Clip(l_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); - _position.Z = Util.Clip(l_position.Z, -100f, 50000f); - d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); - - m_lastposition = _position; - _velocity = Vector3.Zero; - m_lastVelocity = _velocity; - - - if (m_type != Vehicle.TYPE_NONE) - Halt(); - - d.BodySetLinearVel(Body, 0, 0, 0); - base.RequestPhysicsterseUpdate(); - return; - } -*/ base.RequestPhysicsterseUpdate(); if (l_position.Z < 0) @@ -3193,8 +3491,8 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //IsPhysical = false; if (_parent == null) base.RaiseOutOfBounds(_position); - - + + _acceleration.X = 0; // This stuff may stop client display but it has no _acceleration.Y = 0; // effect on the object in phys engine! _acceleration.Z = 0; @@ -3215,13 +3513,13 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); //outofBounds = true; } // end neg Z check - // Is it moving? - /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) - && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) - && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ - if ( (Vector3.Mag(_velocity) < 0.01) && // moving very slowly + // Is it moving? + /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ + if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating - (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001) ) // spinning very slowly + (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly { _zeroFlag = true; m_throttleUpdates = false; @@ -3238,18 +3536,18 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); { // Its stopped _velocity.X = 0.0f; _velocity.Y = 0.0f; - // _velocity.Z = 0.0f; + // _velocity.Z = 0.0f; _acceleration.X = 0; _acceleration.Y = 0; - // _acceleration.Z = 0; - + // _acceleration.Z = 0; + m_rotationalVelocity.X = 0; m_rotationalVelocity.Y = 0; m_rotationalVelocity.Z = 0; // Stop it in the phys engine - d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); + d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); d.BodySetForce(Body, 0f, 0f, 0f); if (!m_lastUpdateSent) @@ -3287,71 +3585,71 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); } } m_lastposition = l_position; - - /// End UpdatePositionAndVelocity insert - - + + /// End UpdatePositionAndVelocity insert + + // Rotation lock ===================================== - if(m_rotateEnableUpdate) - { + if (m_rotateEnableUpdate) + { // Snapshot current angles, set up Amotor(s) m_rotateEnableUpdate = false; m_rotateEnable = m_rotateEnableRequest; -//Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); + //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); if (Amotor != IntPtr.Zero) { d.JointDestroy(Amotor); Amotor = IntPtr.Zero; -//Console.WriteLine("Old Amotor Destroyed"); + //Console.WriteLine("Old Amotor Destroyed"); } - + if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) { // not all are enabled d.Quaternion r = d.BodyGetQuaternion(Body); Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); // extract the axes vectors - Vector3 vX = new Vector3(1f,0f,0f); - Vector3 vY = new Vector3(0f,1f,0f); - Vector3 vZ = new Vector3(0f,0f,1f); - vX = vX * locrot; + Vector3 vX = new Vector3(1f, 0f, 0f); + Vector3 vY = new Vector3(0f, 1f, 0f); + Vector3 vZ = new Vector3(0f, 0f, 1f); + vX = vX * locrot; vY = vY * locrot; vZ = vZ * locrot; // snapshot the current angle vectors m_lockX = vX; m_lockY = vY; m_lockZ = vZ; - // m_lockRot = locrot; + // m_lockRot = locrot; Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); d.JointAttach(Amotor, Body, IntPtr.Zero); d.JointSetAMotorMode(Amotor, 0); // User mode?? -//Console.WriteLine("New Amotor Created for {0}", m_primName); - + //Console.WriteLine("New Amotor Created for {0}", m_primName); + float axisnum = 3; // how many to lock axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); - d.JointSetAMotorNumAxes(Amotor,(int)axisnum); -//Console.WriteLine("AxisNum={0}",(int)axisnum); + d.JointSetAMotorNumAxes(Amotor, (int)axisnum); + //Console.WriteLine("AxisNum={0}",(int)axisnum); int i = 0; if (m_rotateEnable.X == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); -//Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); + //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); i++; } if (m_rotateEnable.Y == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); -//Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); + //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); i++; } if (m_rotateEnable.Z == 0) { d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); -//Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); + //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); i++; } @@ -3362,519 +3660,519 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel3, 0f); - d.JointSetAMotorParam(Amotor, (int) dParam.Vel2, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f); + d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); } // else none are locked } // end Rotation Update - - + + // VEHICLE processing ========================================== - if (m_type != Vehicle.TYPE_NONE) - { - // get body attitude - d.Quaternion rot = d.BodyGetQuaternion(Body); - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object - Quaternion irotq = Quaternion.Inverse(rotq); - - // VEHICLE Linear Motion - d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame - Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); - m_lLinObjectVel = vel_now * irotq; - if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate - { - if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f) - { - float decayfactor = m_linearMotorDecayTimescale/timestep; - Vector3 decayAmount = (m_lLinMotorDVel/decayfactor); - m_lLinMotorDVel -= decayAmount; - } - else - { - float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); - Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; - m_lLinMotorDVel -= decel; - } - if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_lLinMotorDVel = Vector3.Zero; - } - - /* else - { - if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; - if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; - if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; - } */ - } // end linear motor decay - - if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - if (m_linearMotorTimescale < 300.0f) - { - Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; - float linfactor = m_linearMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/linfactor) * 1.3f; - m_lLinObjectVel += attackAmount; - } - if (m_linearFrictionTimescale.X < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.X / timestep; - float fricX = m_lLinObjectVel.X / fricfactor; - m_lLinObjectVel.X -= fricX; - } - if (m_linearFrictionTimescale.Y < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Y / timestep; - float fricY = m_lLinObjectVel.Y / fricfactor; - m_lLinObjectVel.Y -= fricY; - } - if (m_linearFrictionTimescale.Z < 300.0f) - { - float fricfactor = m_linearFrictionTimescale.Z / timestep; - float fricZ = m_lLinObjectVel.Z / fricfactor; - m_lLinObjectVel.Z -= fricZ; - } - } - m_wLinObjectVel = m_lLinObjectVel * rotq; - - // Gravity and Buoyancy - Vector3 grav = Vector3.Zero; - if(m_VehicleBuoyancy < 1.0f) - { - // There is some gravity, make a gravity force vector - // that is applied after object velocity. - d.Mass objMass; - d.BodyGetMass(Body, out objMass); - // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; - grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force - } // else its 1.0, no gravity. - - // Hovering - if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) - { - // We should hover, get the target height - d.Vector3 pos = d.BodyGetPosition(Body); - if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) - { - m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) - { - m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; - } - else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) - { - m_VhoverTargetHeight = m_VhoverHeight; - } - - if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) - { - // If body is aready heigher, use its height as target height - if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; - } - -// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped -// m_VhoverTimescale = 0f; // time to acheive height -// timestep is time since last frame,in secs - float herr0 = pos.Z - m_VhoverTargetHeight; - // Replace Vertical speed with correction figure if significant - if(Math.Abs(herr0) > 0.01f ) - { - //? d.Mass objMass; - //? d.BodyGetMass(Body, out objMass); - m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale); - //KF: m_VhoverEfficiency is not yet implemented - } - else - { - m_wLinObjectVel.Z = 0f; - } - } - else - { // not hovering - if (m_wLinObjectVel.Z == 0f) - { // Gravity rules - m_wLinObjectVel.Z = vel_now.Z; - } // else the motor has it - } - linvel = m_wLinObjectVel; - - // Vehicle Linear Motion done ======================================= - // Apply velocity - d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); - // apply gravity force - d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); -//if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); - // end MoveLinear() - - - // MoveAngular - /* - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - - private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL - private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL - private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL - - private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor - private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body - */ -//if(frcount == 0) Console.WriteLine("MoveAngular "); + if (m_type != Vehicle.TYPE_NONE) + { + // get body attitude + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + Quaternion irotq = Quaternion.Inverse(rotq); + + // VEHICLE Linear Motion + d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame + Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); + m_lLinObjectVel = vel_now * irotq; + if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate + { + if (Vector3.Mag(m_lLinMotorDVel) < 1.0f) + { + float decayfactor = m_linearMotorDecayTimescale / timestep; + Vector3 decayAmount = (m_lLinMotorDVel / decayfactor); + m_lLinMotorDVel -= decayAmount; + } + else + { + float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); + Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; + m_lLinMotorDVel -= decel; + } + if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lLinMotorDVel = Vector3.Zero; + } + + /* else + { + if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; + if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; + if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; + } */ + } // end linear motor decay + + if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + if (m_linearMotorTimescale < 300.0f) + { + Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; + float linfactor = m_linearMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / linfactor) * 1.3f; + m_lLinObjectVel += attackAmount; + } + if (m_linearFrictionTimescale.X < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.X / timestep; + float fricX = m_lLinObjectVel.X / fricfactor; + m_lLinObjectVel.X -= fricX; + } + if (m_linearFrictionTimescale.Y < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Y / timestep; + float fricY = m_lLinObjectVel.Y / fricfactor; + m_lLinObjectVel.Y -= fricY; + } + if (m_linearFrictionTimescale.Z < 300.0f) + { + float fricfactor = m_linearFrictionTimescale.Z / timestep; + float fricZ = m_lLinObjectVel.Z / fricfactor; + m_lLinObjectVel.Z -= fricZ; + } + } + m_wLinObjectVel = m_lLinObjectVel * rotq; + + // Gravity and Buoyancy + Vector3 grav = Vector3.Zero; + if (m_VehicleBuoyancy < 1.0f) + { + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force + } // else its 1.0, no gravity. + + // Hovering + if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + d.Vector3 pos = d.BodyGetPosition(Body); + if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; + } + else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) + { + // If body is aready heigher, use its height as target height + if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + + // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped + // m_VhoverTimescale = 0f; // time to acheive height + // timestep is time since last frame,in secs + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if (Math.Abs(herr0) > 0.01f) + { + //? d.Mass objMass; + //? d.BodyGetMass(Body, out objMass); + m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_wLinObjectVel.Z = 0f; + } + } + else + { // not hovering + if (m_wLinObjectVel.Z == 0f) + { // Gravity rules + m_wLinObjectVel.Z = vel_now.Z; + } // else the motor has it + } + linvel = m_wLinObjectVel; + + // Vehicle Linear Motion done ======================================= + // Apply velocity + d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); + // apply gravity force + d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); + //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); + // end MoveLinear() + + + // MoveAngular + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); - Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); - angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation - -//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); - - // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. - float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); - m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; - // Decay Angular Motor 2. - if (m_angularMotorDecayTimescale < 300.0f) - { - if ( Vector3.Mag(m_angularMotorDVel) < 1.0f) - { - float decayfactor = (m_angularMotorDecayTimescale)/timestep; - Vector3 decayAmount = (m_angularMotorDVel/decayfactor); - m_angularMotorDVel -= decayAmount; - } - else - { - Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; - m_angularMotorDVel -= decel; - } - - if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) - { - m_angularMotorDVel = Vector3.Zero; - } - else - { - if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; - if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; - if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; - } - } // end decay angular motor -//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); - -//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); - - if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) ) - { // if motor or object have motion - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); - - if (m_angularMotorTimescale < 300.0f) - { - Vector3 attack_error = m_angularMotorDVel - angObjectVel; - float angfactor = m_angularMotorTimescale/timestep; - Vector3 attackAmount = (attack_error/angfactor); - angObjectVel += attackAmount; -//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); -//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); - } - - angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); - angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); - angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); - } // else no signif. motion - -//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); - // Bank section tba - // Deflection section tba -//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); - - - /* // Rotation Axis Disables: - if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) - { - if (m_angularEnable.X == 0) - angObjectVel.X = 0f; - if (m_angularEnable.Y == 0) - angObjectVel.Y = 0f; - if (m_angularEnable.Z == 0) - angObjectVel.Z = 0f; - } - */ - angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation - - // Vertical attractor section - Vector3 vertattr = Vector3.Zero; - - if(m_verticalAttractionTimescale < 300) - { - float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); - // make a vector pointing up - Vector3 verterr = Vector3.Zero; - verterr.Z = 1.0f; - // rotate it to Body Angle - verterr = verterr * rotq; - // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. - // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go - // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. - - if (verterr.Z < 0.0f) - { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to - // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. -//Console.WriteLine("InvertFlip"); - verterr.X = 2.0f - verterr.X; - verterr.Y = 2.0f - verterr.Y; - } - verterr *= 0.5f; - // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) - Vector3 xyav = angObjectVel; - xyav.Z = 0.0f; - if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) - { - // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so - // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. - vertattr.X = verterr.Y; - vertattr.Y = - verterr.X; - vertattr.Z = 0f; -//if(frcount == 0) Console.WriteLine("VAerr=" + verterr); - - // scaling appears better usingsquare-law - float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; - float bounce = 1.0f - damped; - // 0 = crit damp, 1 = bouncy - float oavz = angObjectVel.Z; // retain z velocity - // time-scaled correction, which sums, therefore is bouncy: - angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; - // damped, good @ < 90: - angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); - angObjectVel.Z = oavz; -//if(frcount == 0) Console.WriteLine("VA+"); -//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); - } - else - { - // else error is very small - angObjectVel.X = 0f; - angObjectVel.Y = 0f; -//if(frcount == 0) Console.WriteLine("VA0"); - } - } // else vertical attractor is off -//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); - - - m_lastAngularVelocity = angObjectVel; - // apply Angular Velocity to body - d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); -//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); - - } // end VEHICLES - else - { - // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ - - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - - /// Dynamics Buoyancy - //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // NB Prims in ODE are no subject to global gravity - // This should only affect gravity operations - - float m_mass = CalculateMass(); - // calculate z-force due togravity on object. - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass - if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. - { - fz = 0; // llMoveToTarget ignores gravity. - // it also ignores mass of object, and any physical resting on it. - // Vector3 m_PIDTarget is where we are going - // float m_PIDTau is time to get there + private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL + private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL + + private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor + private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body + */ + //if(frcount == 0) Console.WriteLine("MoveAngular "); + + d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); + Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); + angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation + + //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); + + // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. + float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); + m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; + // Decay Angular Motor 2. + if (m_angularMotorDecayTimescale < 300.0f) + { + if (Vector3.Mag(m_angularMotorDVel) < 1.0f) + { + float decayfactor = (m_angularMotorDecayTimescale) / timestep; + Vector3 decayAmount = (m_angularMotorDVel / decayfactor); + m_angularMotorDVel -= decayAmount; + } + else + { + Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; + m_angularMotorDVel -= decel; + } + + if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_angularMotorDVel = Vector3.Zero; + } + else + { + if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; + if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; + if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; + } + } // end decay angular motor + //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); + + //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); + + if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) + { // if motor or object have motion + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); + + if (m_angularMotorTimescale < 300.0f) + { + Vector3 attack_error = m_angularMotorDVel - angObjectVel; + float angfactor = m_angularMotorTimescale / timestep; + Vector3 attackAmount = (attack_error / angfactor); + angObjectVel += attackAmount; + //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); + //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); + } + + angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); + angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); + angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); + } // else no signif. motion + + //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); + // Bank section tba + // Deflection section tba + //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); + + + /* // Rotation Axis Disables: + if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) + { + if (m_angularEnable.X == 0) + angObjectVel.X = 0f; + if (m_angularEnable.Y == 0) + angObjectVel.Y = 0f; + if (m_angularEnable.Z == 0) + angObjectVel.Z = 0f; + } + */ + angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + + if (verterr.Z < 0.0f) + { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to + // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. + //Console.WriteLine("InvertFlip"); + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + verterr *= 0.5f; + // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) + Vector3 xyav = angObjectVel; + xyav.Z = 0.0f; + if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) + { + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = -verterr.X; + vertattr.Z = 0f; + //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // scaling appears better usingsquare-law + float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; + float bounce = 1.0f - damped; + // 0 = crit damp, 1 = bouncy + float oavz = angObjectVel.Z; // retain z velocity + // time-scaled correction, which sums, therefore is bouncy: + angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; + // damped, good @ < 90: + angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); + angObjectVel.Z = oavz; + //if(frcount == 0) Console.WriteLine("VA+"); + //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); + } + else + { + // else error is very small + angObjectVel.X = 0f; + angObjectVel.Y = 0f; + //if(frcount == 0) Console.WriteLine("VA0"); + } + } // else vertical attractor is off + //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); + + + m_lastAngularVelocity = angObjectVel; + // apply Angular Velocity to body + d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); + //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); + + } // end VEHICLES + else + { + // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ + + if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 + + /// Dynamics Buoyancy + //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // NB Prims in ODE are no subject to global gravity + // This should only affect gravity operations + + float m_mass = CalculateMass(); + // calculate z-force due togravity on object. + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass + if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. + { + fz = 0; // llMoveToTarget ignores gravity. + // it also ignores mass of object, and any physical resting on it. + // Vector3 m_PIDTarget is where we are going + // float m_PIDTau is time to get there fx = 0; - fy = 0; - d.Vector3 pos = d.BodyGetPosition(Body); - Vector3 error = new Vector3( - (m_PIDTarget.X - pos.X), - (m_PIDTarget.Y - pos.Y), - (m_PIDTarget.Z - pos.Z)); - if (error.ApproxEquals(Vector3.Zero,0.01f)) - { // Very close, Jump there and quit move - - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - _target_velocity = Vector3.Zero; + fy = 0; + d.Vector3 pos = d.BodyGetPosition(Body); + Vector3 error = new Vector3( + (m_PIDTarget.X - pos.X), + (m_PIDTarget.Y - pos.Y), + (m_PIDTarget.Z - pos.Z)); + if (error.ApproxEquals(Vector3.Zero, 0.01f)) + { // Very close, Jump there and quit move + + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + _target_velocity = Vector3.Zero; d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); d.BodySetForce(Body, 0f, 0f, 0f); - } - else - { - float scale = 50.0f * timestep / m_PIDTau; - if ((error.ApproxEquals(Vector3.Zero,0.5f)) && (_target_velocity != Vector3.Zero)) - { - // Nearby, quit update of velocity - } - else - { // Far, calc damped velocity - _target_velocity = error * scale; - } - d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); } - } // end PID MoveToTarget - - - /// Dynamics Hover =================================================================================== - // Hover PID Controller can only run if the PIDcontroller is not in use. - if (m_useHoverPID && !m_usePID) - { -//Console.WriteLine("Hover " + m_primName); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - - - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); -// d.Vector3 vel = d.BodyGetLinearVel(Body); - - - // 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 = + else + { + float scale = 50.0f * timestep / m_PIDTau; + if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero)) + { + // Nearby, quit update of velocity + } + else + { // Far, calc damped velocity + _target_velocity = error * scale; + } + d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); + } + } // end PID MoveToTarget + + + /// Dynamics Hover =================================================================================== + // Hover PID Controller can only run if the PIDcontroller is not in use. + if (m_useHoverPID && !m_usePID) + { + //Console.WriteLine("Hover " + m_primName); + + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + // d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // 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.Vector3 dlinvel = vel; - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); - d.BodyAddForce(Body, 0, 0, fz); - //KF this prevents furthur motions return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end m_useHoverPID && !m_usePID - - - /// Dynamics Apply Forces =================================================================================== - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - // A physical body at rest on a surface will auto-disable after a while, - // this appears to re-enable it incase the surface it is upon vanishes, - // and the body should fall again. - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0f, 0f, 0f); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; - - - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); - } // end apply forces - } // end Vehicle/Dynamics - - /// RotLookAt / LookAt ================================================================================= + (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.Vector3 dlinvel = vel; + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); + d.BodyAddForce(Body, 0, 0, fz); + //KF this prevents furthur motions return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end m_useHoverPID && !m_usePID + + + /// Dynamics Apply Forces =================================================================================== + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0f, 0f, 0f); + enableBodySoft(); + } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; + + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); + } // end apply forces + } // end Vehicle/Dynamics + + /// RotLookAt / LookAt ================================================================================= if (m_useAPID) { - // RotLookAt, apparently overrides all other rotation sources. Inputs: - // Quaternion m_APIDTarget - // float m_APIDStrength // From SL experiments, this is the time to get there - // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly - // Also in SL the mass of the object has no effect on time to get there. - // Factors: - // get present body rotation - float limit = 1.0f; - float rscaler = 50f; // adjusts rotation damping time - float lscaler = 10f; // adjusts linear damping time in llLookAt - float RLAservo = 0f; + // RotLookAt, apparently overrides all other rotation sources. Inputs: + // Quaternion m_APIDTarget + // float m_APIDStrength // From SL experiments, this is the time to get there + // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly + // Also in SL the mass of the object has no effect on time to get there. + // Factors: + // get present body rotation + float limit = 1.0f; + float rscaler = 50f; // adjusts rotation damping time + float lscaler = 10f; // adjusts linear damping time in llLookAt + float RLAservo = 0f; Vector3 diff_axis; float diff_angle; - d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation - Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); - Quaternion rtarget = new Quaternion(); - - if(m_APIDTarget.W == -99.9f) - { - // this is really a llLookAt(), x,y,z is the target vector - Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); - Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; + d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Quaternion rtarget = new Quaternion(); + + if (m_APIDTarget.W == -99.9f) + { + // this is really a llLookAt(), x,y,z is the target vector + Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); + Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); float twopi = 2.0f * (float)Math.PI; - Vector3 dir = target - _position; - dir.Normalize(); + Vector3 dir = target - _position; + dir.Normalize(); float tzrot = (float)Math.Atan2(dir.Y, dir.X); float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); float terot = (float)Math.Atan2(dir.Z, txy); @@ -3882,63 +4180,63 @@ Console.WriteLine("ODEPrim JointCreateFixed !!!"); float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); float oerot = (float)Math.Atan2(ospin.Z, oxy); float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); - float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y)+(rotq.X * rotq.X)); + float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X)); float roll = (float)Math.Atan2(ra, rb); float errorz = tzrot - ozrot; - if(errorz > (float)Math.PI) errorz -= twopi; - else if(errorz < -(float)Math.PI) errorz += twopi; + if (errorz > (float)Math.PI) errorz -= twopi; + else if (errorz < -(float)Math.PI) errorz += twopi; float errory = oerot - terot; - if(errory > (float)Math.PI) errory -= twopi; - else if(errory < -(float)Math.PI) errory += twopi; + if (errory > (float)Math.PI) errory -= twopi; + else if (errory < -(float)Math.PI) errory += twopi; diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); - if(diff_angle > 0.01f * m_APIDdamper) - { - m_APIDdamper = 1.0f; - RLAservo = timestep / m_APIDStrength * rscaler; + if (diff_angle > 0.01f * m_APIDdamper) + { + m_APIDdamper = 1.0f; + RLAservo = timestep / m_APIDStrength * rscaler; errorz *= RLAservo; errory *= RLAservo; error.X = -roll * 8.0f; error.Y = errory; error.Z = errorz; error *= rotq; - d.BodySetAngularVel (Body, error.X, error.Y, error.Z); - } - else - { - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } - else - { - // this is a llRotLookAt() - rtarget = m_APIDTarget; - - Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot - rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle -//if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); - - // diff_axis.Normalize(); it already is! - if(diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error - { - m_APIDdamper = 1.0f; - Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); - rotforce = rotforce * rotq; - if(diff_angle > limit) diff_angle = limit; // cap the rotate rate - RLAservo = timestep / m_APIDStrength * lscaler; - rotforce = rotforce * RLAservo * diff_angle ; - d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z); -//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); - } - else - { // close enough - d.BodySetAngularVel (Body, 0.0f, 0.0f, 0.0f); - m_APIDdamper = 2.0f; - } - } // end llLookAt/llRotLookAt -//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); + d.BodySetAngularVel(Body, error.X, error.Y, error.Z); + } + else + { + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } + else + { + // this is a llRotLookAt() + rtarget = m_APIDTarget; + + Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot + rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle + //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); + + // diff_axis.Normalize(); it already is! + if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error + { + m_APIDdamper = 1.0f; + Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); + rotforce = rotforce * rotq; + if (diff_angle > limit) diff_angle = limit; // cap the rotate rate + RLAservo = timestep / m_APIDStrength * lscaler; + rotforce = rotforce * RLAservo * diff_angle; + d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z); + //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); + } + else + { // close enough + d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); + m_APIDdamper = 2.0f; + } + } // end llLookAt/llRotLookAt + //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); } // end m_useAPID } // end root prims - } // end Move() - } // end class -} + } // end Move() + } // end class +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index cf7c1d7..03059f7 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -1736,6 +1736,23 @@ namespace OpenSim.Region.Physics.OdePlugin return newPrim; } + private PhysicsActor AddPrim(String name, Vector3 position, PhysicsActor parent, + PrimitiveBaseShape pbs, uint localid, byte[] sdata) + { + Vector3 pos = position; + + OdePrim newPrim; + lock (OdeLock) + { + newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata); + lock (_prims) + _prims.Add(newPrim); + } + + return newPrim; + } + + public void addActivePrim(OdePrim activatePrim) { // adds active prim.. (ones that should be iterated over in collisions_optimized @@ -1762,6 +1779,17 @@ namespace OpenSim.Region.Physics.OdePlugin return result; } + public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + PhysicsActor result; + + result = AddPrim(primName, position, parent, + pbs, localid, sdata); + + return result; + } + public override float TimeDilation { get { return m_timeDilation; } diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs new file mode 100644 index 0000000..edd58d3 --- /dev/null +++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs @@ -0,0 +1,167 @@ +// adapted from libomv removing cpu endian adjust +// for prims lowlevel serialization + +using System; +using System.IO; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class wstreamer + { + private MemoryStream st; + + public wstreamer() + { + st = new MemoryStream(); + } + + public byte[] close() + { + byte[] data = st.ToArray(); + st.Close(); + return data; + } + + public void Wshort(short value) + { + st.Write(BitConverter.GetBytes(value), 0, 2); + } + public void Wushort(ushort value) + { + byte[] t = BitConverter.GetBytes(value); + st.Write(BitConverter.GetBytes(value), 0, 2); + } + public void Wint(int value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + public void Wuint(uint value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + public void Wlong(long value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + public void Wulong(ulong value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + + public void Wfloat(float value) + { + st.Write(BitConverter.GetBytes(value), 0, 4); + } + + public void Wdouble(double value) + { + st.Write(BitConverter.GetBytes(value), 0, 8); + } + + public void Wvector3(Vector3 value) + { + st.Write(BitConverter.GetBytes(value.X), 0, 4); + st.Write(BitConverter.GetBytes(value.Y), 0, 4); + st.Write(BitConverter.GetBytes(value.Z), 0, 4); + } + public void Wquat(Quaternion value) + { + st.Write(BitConverter.GetBytes(value.X), 0, 4); + st.Write(BitConverter.GetBytes(value.Y), 0, 4); + st.Write(BitConverter.GetBytes(value.Z), 0, 4); + st.Write(BitConverter.GetBytes(value.W), 0, 4); + } + } + + public class rstreamer + { + private byte[] rbuf; + private int ptr; + + public rstreamer(byte[] data) + { + rbuf = data; + ptr = 0; + } + + public void close() + { + } + + public short Rshort() + { + short v = BitConverter.ToInt16(rbuf, ptr); + ptr += 2; + return v; + } + public ushort Rushort() + { + ushort v = BitConverter.ToUInt16(rbuf, ptr); + ptr += 2; + return v; + } + public int Rint() + { + int v = BitConverter.ToInt32(rbuf, ptr); + ptr += 4; + return v; + } + public uint Ruint() + { + uint v = BitConverter.ToUInt32(rbuf, ptr); + ptr += 4; + return v; + } + public long Rlong() + { + long v = BitConverter.ToInt64(rbuf, ptr); + ptr += 8; + return v; + } + public ulong Rulong() + { + ulong v = BitConverter.ToUInt64(rbuf, ptr); + ptr += 8; + return v; + } + public float Rfloat() + { + float v = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + + public double Rdouble() + { + double v = BitConverter.ToDouble(rbuf, ptr); + ptr += 8; + return v; + } + + public Vector3 Rvector3() + { + Vector3 v; + v.X = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Y = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Z = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + public Quaternion Rquat() + { + Quaternion v; + v.X = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Y = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.Z = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + v.W = BitConverter.ToSingle(rbuf, ptr); + ptr += 4; + return v; + } + } +} diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index e1a68be..eb0228a 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -214,6 +214,11 @@ namespace OpenSim.Region.Physics.Manager } } + public virtual byte[] Serialize(bool PhysIsRunning) + { + return new byte[0]; + } + public virtual void RaiseOutOfBounds(Vector3 pos) { // Make a temporary copy of the event to avoid possibility of @@ -573,5 +578,6 @@ namespace OpenSim.Region.Physics.Manager { return false; } + } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsScene.cs b/OpenSim/Region/Physics/Manager/PhysicsScene.cs index 3db71e5..0346d4e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsScene.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsScene.cs @@ -128,6 +128,12 @@ namespace OpenSim.Region.Physics.Manager public abstract PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, Vector3 size, Quaternion rotation, bool isPhysical, uint localid); + public virtual PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position, + uint localid, byte[] sdata) + { + return null; + } + public virtual float TimeDilation { get { return 1.0f; } -- cgit v1.1 From 5351ff925c20c0fd189ee7de85c31521aca8cdf3 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 14:08:42 +0000 Subject: let SOG know about vehicles. Still needs serialization and applyphyscis on deserialize, etc --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 431 +++++++++++++++++++++ .../Region/Framework/Scenes/SceneObjectGroup.cs | 98 ++++- OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 42 +- 3 files changed, 541 insertions(+), 30 deletions(-) create mode 100644 OpenSim/Region/Framework/Scenes/SOGVehicle.cs (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs new file mode 100644 index 0000000..d6e9a3a --- /dev/null +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -0,0 +1,431 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + + +namespace OpenSim.Region.Framework.Scenes +{ + public class SOGVehicle + { + public Vehicle Type + { + get { return m_type; } + } + private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? + + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + + private VehicleFlag m_flags = (VehicleFlag)0; + private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl + + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + private float m_linearMotorDecayTimescale = 120; + private float m_linearMotorTimescale = 1000; + private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate + + //Deflection properties + private float m_angularDeflectionEfficiency = 0; + private float m_angularDeflectionTimescale = 1000; + private float m_linearDeflectionEfficiency = 0; + private float m_linearDeflectionTimescale = 1000; + + //Banking properties + private float m_bankingEfficiency = 0; + private float m_bankingMix = 0; + private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; + private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 1000f; + private float m_VehicleBuoyancy = 0f; + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. + + public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + 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; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + m_linearMotorDecayTimescale = pValue; + 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); + 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); + 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 + + public void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + 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); + 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); + 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 + + public 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 + + public void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + m_flags &= ~((VehicleFlag)pParam); + } + else + { + m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + public void ProcessTypeChange(Vehicle pType) + { + 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: // none sense this will never exist + m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 1000; + 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; + 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 = 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; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 1; + m_linearDeflectionTimescale = 2; + m_angularDeflectionEfficiency = 0; + m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + m_bankingEfficiency = -0.2f; + m_bankingMix = 1; + m_bankingTimescale = 1; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 3; + m_angularDeflectionEfficiency = 0.5f; + m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + m_bankingEfficiency = -0.3f; + m_bankingMix = 0.8f; + m_bankingTimescale = 1; + m_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; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 8; + m_VhoverHeight = 0; + m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + m_linearDeflectionEfficiency = 0.5f; + m_linearDeflectionTimescale = 0.5f; + m_angularDeflectionEfficiency = 1; + m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + m_bankingEfficiency = 1; + m_bankingMix = 0.7f; + m_bankingTimescale = 2; + m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_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 = 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; + } + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 53edcd6..1bac4d8 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -43,6 +43,7 @@ using OpenSim.Region.Framework.Scenes.Serialization; namespace OpenSim.Region.Framework.Scenes { + [Flags] public enum scriptEvents { @@ -114,6 +115,90 @@ namespace OpenSim.Region.Framework.Scenes private bool m_suspendUpdates; private List m_linkedAvatars = new List(); + private SOGVehicle m_vehicle = null; + + public int VehicleType + { + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + m_vehicle = null; + if (value == (int)Vehicle.TYPE_NONE) + { + if (RootPart.PhysActor != null) + RootPart.PhysActor.VehicleType = (int)Vehicle.TYPE_NONE; + return; + } + m_vehicle = new SOGVehicle(); + m_vehicle.ProcessTypeChange((Vehicle)value); + { + if (RootPart.PhysActor != null) + RootPart.PhysActor.VehicleType = value; + return; + } + + } + } + + public void SetVehicleFlags(int param, bool remove) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessVehicleFlags(param, remove); + + if (RootPart.PhysActor != null) + { + RootPart.PhysActor.VehicleFlags(param, remove); + } + } + + public void SetVehicleFloatParam(int param, float value) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + + if (RootPart.PhysActor != null) + { + RootPart.PhysActor.VehicleFloatParam(param, value); + } + } + + public void SetVehicleVectorParam(int param, Vector3 value) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + + if (RootPart.PhysActor != null) + { + RootPart.PhysActor.VehicleVectorParam(param, value); + } + } + + public void SetVehicleRotationParam(int param, Quaternion rotation) + { + if (m_vehicle == null) + return; + + m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + + if (RootPart.PhysActor != null) + { + RootPart.PhysActor.VehicleRotationParam(param, rotation); + } + } + + public bool areUpdatesSuspended { get @@ -1678,10 +1763,6 @@ namespace OpenSim.Region.Framework.Scenes /// public void ApplyPhysics() { - // Apply physics to the root prim - // m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive); - - // Apply physics to child prims SceneObjectPart[] parts = m_parts.GetArray(); if (parts.Length > 1) { @@ -1689,18 +1770,21 @@ namespace OpenSim.Region.Framework.Scenes // Apply physics to the root prim m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, true); + + for (int i = 0; i < parts.Length; i++) { SceneObjectPart part = parts[i]; if (part.LocalId != m_rootPart.LocalId) -// part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive); part.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), part.VolumeDetectActive, true); - } // Hack to get the physics scene geometries in the right spot // ResetChildPrimPhysicsPositions(); - if (m_rootPart.PhysActor != null) + if (m_rootPart.PhysActor != null) + { m_rootPart.PhysActor.Building = false; + + } } else { diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 19dedde..6438694 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -3170,34 +3170,37 @@ namespace OpenSim.Region.Framework.Scenes public void SetVehicleType(int type) { - if (PhysActor != null) - { - PhysActor.VehicleType = type; - } + if (ParentGroup.IsDeleted) + return; + ParentGroup.VehicleType = type; + } + + public void SetVehicleFlags(int param, bool remove) + { + if (ParentGroup.IsDeleted) + return; + ParentGroup.SetVehicleFlags(param, remove); } public void SetVehicleFloatParam(int param, float value) { - if (PhysActor != null) - { - PhysActor.VehicleFloatParam(param, value); - } + if (ParentGroup.IsDeleted) + return; + ParentGroup.SetVehicleFloatParam(param, value); } public void SetVehicleVectorParam(int param, Vector3 value) { - if (PhysActor != null) - { - PhysActor.VehicleVectorParam(param, value); - } + if (ParentGroup.IsDeleted) + return; + ParentGroup.SetVehicleVectorParam(param, value); } public void SetVehicleRotationParam(int param, Quaternion rotation) { - if (PhysActor != null) - { - PhysActor.VehicleRotationParam(param, rotation); - } + if (ParentGroup.IsDeleted) + return; + ParentGroup.SetVehicleRotationParam(param, rotation); } /// @@ -3381,13 +3384,6 @@ namespace OpenSim.Region.Framework.Scenes hasProfileCut = hasDimple; // is it the same thing? } - public void SetVehicleFlags(int param, bool remove) - { - if (PhysActor != null) - { - PhysActor.VehicleFlags(param, remove); - } - } public void SetGroup(UUID groupID, IClientAPI client) { -- cgit v1.1 From 91a326331fedc702cc3f04a628841447b90d009a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 16:16:48 +0000 Subject: vehicle parameters do cross (i hope) on regions in same instance ( others need xml) --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 63 +++++++++++++++++----- .../Region/Framework/Scenes/SceneObjectGroup.cs | 7 ++- 2 files changed, 57 insertions(+), 13 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index d6e9a3a..b7c93e5 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -39,13 +39,9 @@ namespace OpenSim.Region.Framework.Scenes { get { return m_type; } } - private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier - private Quaternion m_RollreferenceFrame = Quaternion.Identity; // what hell is this ? private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - private VehicleFlag m_flags = (VehicleFlag)0; - private Vector3 m_BlockingEndPoint = Vector3.Zero; // not sl // Linear properties private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time @@ -81,6 +77,9 @@ namespace OpenSim.Region.Framework.Scenes private float m_verticalAttractionEfficiency = 1.0f; // damped private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. + // Axis + public Quaternion m_referenceFrame = Quaternion.Identity; + public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { float len; @@ -235,9 +234,6 @@ namespace OpenSim.Region.Framework.Scenes 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 @@ -248,9 +244,6 @@ namespace OpenSim.Region.Framework.Scenes case Vehicle.REFERENCE_FRAME: m_referenceFrame = Quaternion.Inverse(pValue); break; - case Vehicle.ROLL_FRAME: - m_RollreferenceFrame = pValue; - break; } }//end ProcessRotationVehicleParam @@ -271,8 +264,6 @@ namespace OpenSim.Region.Framework.Scenes m_linearMotorDirection = Vector3.Zero; m_angularMotorDirection = Vector3.Zero; - m_BlockingEndPoint = Vector3.Zero; - m_RollreferenceFrame = Quaternion.Identity; m_linearMotorOffset = Vector3.Zero; m_referenceFrame = Quaternion.Identity; @@ -427,5 +418,53 @@ namespace OpenSim.Region.Framework.Scenes break; } } + public void SetVehicle(PhysicsActor ph) + { + + // crap crap crap + if (ph == null) // what ?? + return; + + ph.VehicleType = (int)m_type; + + // Linear properties + ph.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_DIRECTION, m_linearMotorDirection); + ph.VehicleVectorParam((int)Vehicle.LINEAR_FRICTION_TIMESCALE, m_linearFrictionTimescale); + ph.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, m_linearMotorDecayTimescale); + ph.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_TIMESCALE, m_linearMotorTimescale); + ph.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_OFFSET, m_linearMotorOffset); + + //Angular properties + ph.VehicleVectorParam((int)Vehicle.ANGULAR_MOTOR_DIRECTION, m_angularMotorDirection); + ph.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_TIMESCALE, m_angularMotorTimescale); + ph.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, m_angularMotorDecayTimescale); + ph.VehicleVectorParam((int)Vehicle.ANGULAR_FRICTION_TIMESCALE, m_angularFrictionTimescale); + + //Deflection properties + ph.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, m_angularDeflectionEfficiency); + ph.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_TIMESCALE, m_angularDeflectionTimescale); + ph.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_EFFICIENCY, m_linearDeflectionEfficiency); + ph.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_TIMESCALE, m_linearDeflectionTimescale); + + //Banking properties + ph.VehicleFloatParam((int)Vehicle.BANKING_EFFICIENCY, m_bankingEfficiency); + ph.VehicleFloatParam((int)Vehicle.BANKING_MIX, m_bankingMix); + ph.VehicleFloatParam((int)Vehicle.BANKING_TIMESCALE, m_bankingTimescale); + + //Hover and Buoyancy properties + ph.VehicleFloatParam((int)Vehicle.HOVER_HEIGHT, m_VhoverHeight); + ph.VehicleFloatParam((int)Vehicle.HOVER_EFFICIENCY, m_VhoverEfficiency); + ph.VehicleFloatParam((int)Vehicle.HOVER_TIMESCALE, m_VhoverTimescale); + ph.VehicleFloatParam((int)Vehicle.BUOYANCY, m_VehicleBuoyancy); + + //Attractor properties + ph.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, m_verticalAttractionEfficiency); + ph.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_TIMESCALE, m_verticalAttractionTimescale); + + ph.VehicleRotationParam((int)Vehicle.REFERENCE_FRAME, m_referenceFrame); + + ph.VehicleFlags(~(int)m_flags, true); + ph.VehicleFlags((int)m_flags, false); + } } } \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 1bac4d8..324fdb8 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -1782,14 +1782,19 @@ namespace OpenSim.Region.Framework.Scenes // ResetChildPrimPhysicsPositions(); if (m_rootPart.PhysActor != null) { + if (m_vehicle != null) + m_vehicle.SetVehicle(m_rootPart.PhysActor); m_rootPart.PhysActor.Building = false; - } } else { // Apply physics to the root prim m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, false); + if (m_rootPart.PhysActor != null && m_vehicle != null) + { + m_vehicle.SetVehicle(m_rootPart.PhysActor); + } } } -- cgit v1.1 From 3aee642190add7045f78e522ae7b2221b3566f1e Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 17:42:14 +0000 Subject: changed how vehicle data is stored and passed to physics. use unsafe in serializer, tried to control m_dupeInProgress --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 336 +++++++++++---------- .../Region/Framework/Scenes/SceneObjectGroup.cs | 2 + OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 62 +++- OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs | 294 ++++++++++++++---- OpenSim/Region/Physics/Manager/PhysicsActor.cs | 5 +- OpenSim/Region/Physics/Manager/VehicleConstants.cs | 45 ++- 6 files changed, 522 insertions(+), 222 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index b7c93e5..470f403 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -37,9 +37,11 @@ namespace OpenSim.Region.Framework.Scenes { public Vehicle Type { - get { return m_type; } + get { return vd.m_type; } } + public VehicleData vd; +/* private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind private VehicleFlag m_flags = (VehicleFlag)0; @@ -78,7 +80,13 @@ namespace OpenSim.Region.Framework.Scenes private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. // Axis - public Quaternion m_referenceFrame = Quaternion.Identity; + public Quaternion m_referenceFrame = Quaternion.Identity; +*/ + public SOGVehicle() + { + vd = new VehicleData(); + ProcessTypeChange(Vehicle.TYPE_NONE); // is needed? + } public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) { @@ -89,109 +97,109 @@ namespace OpenSim.Region.Framework.Scenes case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: if (pValue < 0f) pValue = 0f; if (pValue > 1f) pValue = 1f; - m_angularDeflectionEfficiency = pValue; + vd.m_angularDeflectionEfficiency = pValue; break; case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_angularDeflectionTimescale = pValue; + vd.m_angularDeflectionTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: if (pValue < timestep) pValue = timestep; else if (pValue > 120) pValue = 120; - m_angularMotorDecayTimescale = pValue; + vd.m_angularMotorDecayTimescale = pValue; break; case Vehicle.ANGULAR_MOTOR_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_angularMotorTimescale = pValue; + vd.m_angularMotorTimescale = pValue; break; case Vehicle.BANKING_EFFICIENCY: if (pValue < -1f) pValue = -1f; if (pValue > 1f) pValue = 1f; - m_bankingEfficiency = pValue; + vd.m_bankingEfficiency = pValue; break; case Vehicle.BANKING_MIX: if (pValue < 0f) pValue = 0f; if (pValue > 1f) pValue = 1f; - m_bankingMix = pValue; + vd.m_bankingMix = pValue; break; case Vehicle.BANKING_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_bankingTimescale = pValue; + vd.m_bankingTimescale = pValue; break; case Vehicle.BUOYANCY: if (pValue < -1f) pValue = -1f; if (pValue > 1f) pValue = 1f; - m_VehicleBuoyancy = pValue; + vd.m_VehicleBuoyancy = pValue; break; case Vehicle.HOVER_EFFICIENCY: if (pValue < 0f) pValue = 0f; if (pValue > 1f) pValue = 1f; - m_VhoverEfficiency = pValue; + vd.m_VhoverEfficiency = pValue; break; case Vehicle.HOVER_HEIGHT: - m_VhoverHeight = pValue; + vd.m_VhoverHeight = pValue; break; case Vehicle.HOVER_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_VhoverTimescale = pValue; + vd.m_VhoverTimescale = pValue; break; case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: if (pValue < 0f) pValue = 0f; if (pValue > 1f) pValue = 1f; - m_linearDeflectionEfficiency = pValue; + vd.m_linearDeflectionEfficiency = pValue; break; case Vehicle.LINEAR_DEFLECTION_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_linearDeflectionTimescale = pValue; + vd.m_linearDeflectionTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: // if (pValue < timestep) pValue = timestep; // try to make impulses to work a bit better if (pValue < timestep) pValue = timestep; else if (pValue > 120) pValue = 120; - m_linearMotorDecayTimescale = pValue; + vd.m_linearMotorDecayTimescale = pValue; break; case Vehicle.LINEAR_MOTOR_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_linearMotorTimescale = pValue; + vd.m_linearMotorTimescale = pValue; break; case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: if (pValue < 0f) pValue = 0f; if (pValue > 1f) pValue = 1f; - m_verticalAttractionEfficiency = pValue; + vd.m_verticalAttractionEfficiency = pValue; break; case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_verticalAttractionTimescale = pValue; + vd.m_verticalAttractionTimescale = pValue; break; // These are vector properties but the engine lets you use a single float value to // set all of the components to the same value case Vehicle.ANGULAR_FRICTION_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + vd.m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.ANGULAR_MOTOR_DIRECTION: - m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - len = m_angularMotorDirection.Length(); + vd.m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_angularMotorDirection.Length(); if (len > 12.566f) - m_angularMotorDirection *= (12.566f / len); + vd.m_angularMotorDirection *= (12.566f / len); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: if (pValue < timestep) pValue = timestep; - m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + vd.m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_MOTOR_DIRECTION: - m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - len = m_linearMotorDirection.Length(); + vd.m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorDirection.Length(); if (len > 30.0f) - m_linearMotorDirection *= (30.0f / len); + vd.m_linearMotorDirection *= (30.0f / len); break; case Vehicle.LINEAR_MOTOR_OFFSET: - m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - len = m_linearMotorOffset.Length(); + vd.m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorOffset.Length(); if (len > 100.0f) - m_linearMotorOffset *= (100.0f / len); + vd.m_linearMotorOffset *= (100.0f / len); break; } }//end ProcessFloatVehicleParam @@ -207,32 +215,32 @@ namespace OpenSim.Region.Framework.Scenes if (pValue.Y < timestep) pValue.Y = timestep; if (pValue.Z < timestep) pValue.Z = timestep; - m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + vd.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); + vd.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(); + len = vd.m_angularMotorDirection.Length(); if (len > 12.566f) - m_angularMotorDirection *= (12.566f / len); + vd.m_angularMotorDirection *= (12.566f / len); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: if (pValue.X < timestep) pValue.X = timestep; if (pValue.Y < timestep) pValue.Y = timestep; if (pValue.Z < timestep) pValue.Z = timestep; - m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + vd.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(); + vd.m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorDirection.Length(); if (len > 30.0f) - m_linearMotorDirection *= (30.0f / len); + vd.m_linearMotorDirection *= (30.0f / len); break; case Vehicle.LINEAR_MOTOR_OFFSET: - m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - len = m_linearMotorOffset.Length(); + vd.m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorOffset.Length(); if (len > 100.0f) - m_linearMotorOffset *= (100.0f / len); + vd.m_linearMotorOffset *= (100.0f / len); break; } }//end ProcessVectorVehicleParam @@ -242,7 +250,7 @@ namespace OpenSim.Region.Framework.Scenes switch (pParam) { case Vehicle.REFERENCE_FRAME: - m_referenceFrame = Quaternion.Inverse(pValue); + vd.m_referenceFrame = Quaternion.Inverse(pValue); break; } }//end ProcessRotationVehicleParam @@ -251,169 +259,169 @@ namespace OpenSim.Region.Framework.Scenes { if (remove) { - m_flags &= ~((VehicleFlag)pParam); + vd.m_flags &= ~((VehicleFlag)pParam); } else { - m_flags |= (VehicleFlag)pParam; + vd.m_flags |= (VehicleFlag)pParam; } }//end ProcessVehicleFlags public void ProcessTypeChange(Vehicle pType) { - m_linearMotorDirection = Vector3.Zero; - m_angularMotorDirection = Vector3.Zero; + vd.m_linearMotorDirection = Vector3.Zero; + vd.m_angularMotorDirection = Vector3.Zero; - m_linearMotorOffset = Vector3.Zero; + vd.m_linearMotorOffset = Vector3.Zero; - m_referenceFrame = Quaternion.Identity; + vd.m_referenceFrame = Quaternion.Identity; // Set Defaults For Type - m_type = pType; + vd.m_type = pType; switch (pType) { case Vehicle.TYPE_NONE: // none sense this will never exist - m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; - m_angularMotorTimescale = 1000; - m_angularMotorDecayTimescale = 1000; - m_VhoverHeight = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - m_flags = (VehicleFlag)0; + vd.m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 1000; + vd.m_VhoverHeight = 0; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_flags = (VehicleFlag)0; break; case Vehicle.TYPE_SLED: - m_linearFrictionTimescale = new Vector3(30, 1, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1000; - m_linearMotorDecayTimescale = 120; - m_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 = 0; - m_angularDeflectionTimescale = 1000; - m_bankingEfficiency = 0; - m_bankingMix = 1; - m_bankingTimescale = 10; - m_flags &= + vd.m_linearFrictionTimescale = new Vector3(30, 1, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 120; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 1; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 10; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); break; case Vehicle.TYPE_CAR: - m_linearFrictionTimescale = new Vector3(100, 2, 1000); - m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - m_linearMotorTimescale = 1; - m_linearMotorDecayTimescale = 60; - m_angularMotorTimescale = 1; - m_angularMotorDecayTimescale = 0.8f; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0; - m_VhoverTimescale = 1000; - m_VehicleBuoyancy = 0; - m_linearDeflectionEfficiency = 1; - m_linearDeflectionTimescale = 2; - m_angularDeflectionEfficiency = 0; - m_angularDeflectionTimescale = 10; - m_verticalAttractionEfficiency = 1f; - m_verticalAttractionTimescale = 10f; - m_bankingEfficiency = -0.2f; - m_bankingMix = 1; - m_bankingTimescale = 1; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + vd.m_linearFrictionTimescale = new Vector3(100, 2, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 1; + vd.m_angularMotorDecayTimescale = 0.8f; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 2; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 10; + vd.m_verticalAttractionEfficiency = 1f; + vd.m_verticalAttractionTimescale = 10f; + vd.m_bankingEfficiency = -0.2f; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); break; case Vehicle.TYPE_BOAT: - m_linearFrictionTimescale = new Vector3(10, 3, 2); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 4; - m_VhoverHeight = 0; - m_VhoverEfficiency = 0.5f; - m_VhoverTimescale = 2; - m_VehicleBuoyancy = 1; - m_linearDeflectionEfficiency = 0.5f; - m_linearDeflectionTimescale = 3; - m_angularDeflectionEfficiency = 0.5f; - m_angularDeflectionTimescale = 5; - m_verticalAttractionEfficiency = 0.5f; - m_verticalAttractionTimescale = 5f; - m_bankingEfficiency = -0.3f; - m_bankingMix = 0.8f; - m_bankingTimescale = 1; - m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + vd.m_linearFrictionTimescale = new Vector3(10, 3, 2); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 4; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 2; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 3; + vd.m_angularDeflectionEfficiency = 0.5f; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0.5f; + vd.m_verticalAttractionTimescale = 5f; + vd.m_bankingEfficiency = -0.3f; + vd.m_bankingMix = 0.8f; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_ROLL_ONLY); - m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + vd.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; - m_angularMotorTimescale = 4; - m_angularMotorDecayTimescale = 8; - 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 | + vd.m_linearFrictionTimescale = new Vector3(200, 10, 5); + vd.m_angularFrictionTimescale = new Vector3(20, 20, 20); + vd.m_linearMotorTimescale = 2; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 8; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 0.5f; + vd.m_angularDeflectionEfficiency = 1; + vd.m_angularDeflectionTimescale = 2; + vd.m_verticalAttractionEfficiency = 0.9f; + vd.m_verticalAttractionTimescale = 2f; + vd.m_bankingEfficiency = 1; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 2; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); break; case Vehicle.TYPE_BALLOON: - m_linearFrictionTimescale = new Vector3(5, 5, 5); - m_angularFrictionTimescale = new Vector3(10, 10, 10); - m_linearMotorTimescale = 5; - m_linearMotorDecayTimescale = 60; - m_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 = 0f; - m_verticalAttractionTimescale = 1000f; - m_bankingEfficiency = 0; - m_bankingMix = 0.7f; - m_bankingTimescale = 5; - m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + vd.m_linearFrictionTimescale = new Vector3(5, 5, 5); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 6; + vd.m_angularMotorDecayTimescale = 10; + vd.m_VhoverHeight = 5; + vd.m_VhoverEfficiency = 0.8f; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 5; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0f; + vd.m_verticalAttractionTimescale = 1000f; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 5; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); - m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); break; } @@ -424,7 +432,8 @@ namespace OpenSim.Region.Framework.Scenes // crap crap crap if (ph == null) // what ?? return; - + ph.SetVehicle(vd); +/* ph.VehicleType = (int)m_type; // Linear properties @@ -465,6 +474,7 @@ namespace OpenSim.Region.Framework.Scenes ph.VehicleFlags(~(int)m_flags, true); ph.VehicleFlags((int)m_flags, false); + */ } } } \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 324fdb8..1cce4c0 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -1965,6 +1965,7 @@ namespace OpenSim.Region.Framework.Scenes /// public SceneObjectGroup Copy(bool userExposed) { + m_dupeInProgress = true; SceneObjectGroup dupe = (SceneObjectGroup)MemberwiseClone(); dupe.m_isBackedUp = false; dupe.m_parts = new MapAndArray(); @@ -2048,6 +2049,7 @@ namespace OpenSim.Region.Framework.Scenes ScheduleGroupForFullUpdate(); } + m_dupeInProgress = false; return dupe; } diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs index 6e28bfa..2306309 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs @@ -22,7 +22,8 @@ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * - * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs + * Ubit 2012 * rolled into ODEPrim.cs */ @@ -38,7 +39,6 @@ using Ode.NET; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; - namespace OpenSim.Region.Physics.OdePlugin { /// @@ -254,6 +254,61 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. SerialControl m_taintserial = null; + object m_taintvehicledata = null; + + public void DoSetVehicle() + { + VehicleData vd = (VehicleData)m_taintvehicledata; + + m_type = vd.m_type; + m_flags = vd.m_flags; + + // Linear properties + m_linearMotorDirection = vd.m_linearMotorDirection; + m_linearFrictionTimescale = vd.m_linearFrictionTimescale; + m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; + m_linearMotorTimescale = vd.m_linearMotorTimescale; +// m_linearMotorOffset = vd.m_linearMotorOffset; + + //Angular properties + m_angularMotorDirection = vd.m_angularMotorDirection; + m_angularMotorTimescale = vd.m_angularMotorTimescale; + m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; + m_angularFrictionTimescale = vd.m_angularFrictionTimescale; + + //Deflection properties +// m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; +// m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; +// m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; +// m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; + + //Banking properties +// m_bankingEfficiency = vd.m_bankingEfficiency; +// m_bankingMix = vd.m_bankingMix; +// m_bankingTimescale = vd.m_bankingTimescale; + + //Hover and Buoyancy properties + m_VhoverHeight = vd.m_VhoverHeight; +// m_VhoverEfficiency = vd.m_VhoverEfficiency; + m_VhoverTimescale = vd.m_VhoverTimescale; + m_VehicleBuoyancy = vd.m_VehicleBuoyancy; + + //Attractor properties + m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; + m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; + + // Axis +// m_referenceFrame = vd.m_referenceFrame; + + + m_taintvehicledata = null; + } + + public override void SetVehicle(object vdata) + { + m_taintvehicledata = vdata; + _parent_scene.AddPhysicsActorTaint(this); + } public override byte[] Serialize(bool PhysIsRunning) { @@ -1843,6 +1898,9 @@ namespace OpenSim.Region.Physics.OdePlugin if (m_taintCollidesWater != m_collidesWater) changefloatonwater(timestep); + if (m_taintvehicledata != null) + DoSetVehicle(); + if (m_taintserial != null) DoSerialize(m_taintserial); diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs index edd58d3..e7e7bb3 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdeUtils.cs @@ -1,5 +1,17 @@ -// adapted from libomv removing cpu endian adjust -// for prims lowlevel serialization +/* Ubit 2012 + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +// no endian conversion. So can't be use to pass information around diferent cpus with diferent endian using System; using System.IO; @@ -7,77 +19,168 @@ using OpenMetaverse; namespace OpenSim.Region.Physics.OdePlugin { - public class wstreamer + + unsafe public class wstreamer { - private MemoryStream st; + byte[] buf; + int index; + byte* src; public wstreamer() { - st = new MemoryStream(); + buf = new byte[1024]; + index = 0; + } + public wstreamer(int size) + { + buf = new byte[size]; + index = 0; } public byte[] close() { - byte[] data = st.ToArray(); - st.Close(); + byte[] data = new byte[index]; + Buffer.BlockCopy(buf, 0, data, 0, index); return data; } + public void Seek(int pos) + { + index = pos; + } + + public void Seekrel(int pos) + { + index += pos; + } + + public void Wbyte(byte value) + { + buf[index++] = value; + } public void Wshort(short value) { - st.Write(BitConverter.GetBytes(value), 0, 2); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; } public void Wushort(ushort value) { - byte[] t = BitConverter.GetBytes(value); - st.Write(BitConverter.GetBytes(value), 0, 2); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src; } public void Wint(int value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wuint(uint value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wlong(long value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wulong(ulong value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wfloat(float value) { - st.Write(BitConverter.GetBytes(value), 0, 4); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wdouble(double value) { - st.Write(BitConverter.GetBytes(value), 0, 8); + src = (byte*)&value; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wvector3(Vector3 value) { - st.Write(BitConverter.GetBytes(value.X), 0, 4); - st.Write(BitConverter.GetBytes(value.Y), 0, 4); - st.Write(BitConverter.GetBytes(value.Z), 0, 4); + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } public void Wquat(Quaternion value) { - st.Write(BitConverter.GetBytes(value.X), 0, 4); - st.Write(BitConverter.GetBytes(value.Y), 0, 4); - st.Write(BitConverter.GetBytes(value.Z), 0, 4); - st.Write(BitConverter.GetBytes(value.W), 0, 4); + src = (byte*)&value.X; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Y; // it may have padding ?? + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.Z; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; + src = (byte*)&value.W; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src++; + buf[index++] = *src; } } - public class rstreamer + unsafe public class rstreamer { private byte[] rbuf; private int ptr; + private byte* dst; public rstreamer(byte[] data) { @@ -89,78 +192,161 @@ namespace OpenSim.Region.Physics.OdePlugin { } + public void Seek(int pos) + { + ptr = pos; + } + + public void Seekrel(int pos) + { + ptr += pos; + } + + public byte Rbyte() + { + return (byte)rbuf[ptr++]; + } + public short Rshort() { - short v = BitConverter.ToInt16(rbuf, ptr); - ptr += 2; + short v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public ushort Rushort() { - ushort v = BitConverter.ToUInt16(rbuf, ptr); - ptr += 2; + ushort v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public int Rint() { - int v = BitConverter.ToInt32(rbuf, ptr); - ptr += 4; + int v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public uint Ruint() { - uint v = BitConverter.ToUInt32(rbuf, ptr); - ptr += 4; + uint v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public long Rlong() { - long v = BitConverter.ToInt64(rbuf, ptr); - ptr += 8; + long v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public ulong Rulong() { - ulong v = BitConverter.ToUInt64(rbuf, ptr); - ptr += 8; + ulong v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public float Rfloat() { - float v = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + float v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public double Rdouble() { - double v = BitConverter.ToDouble(rbuf, ptr); - ptr += 8; + double v; + dst = (byte*)&v; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } public Vector3 Rvector3() { Vector3 v; - v.X = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Y = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Z = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; return v; } + public Quaternion Rquat() { Quaternion v; - v.X = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Y = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.Z = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; - v.W = BitConverter.ToSingle(rbuf, ptr); - ptr += 4; + dst = (byte*)&v.X; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Y; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.Z; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + + dst = (byte*)&v.W; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst++ = rbuf[ptr++]; + *dst = rbuf[ptr++]; + return v; } } diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs index eb0228a..f525e9e 100644 --- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs +++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs @@ -146,6 +146,8 @@ namespace OpenSim.Region.Physics.Manager /// public event CollisionUpdate OnCollisionUpdate; + public virtual void SetVehicle(object vdata) { } + public event OutOfBounds OnOutOfBounds; #pragma warning restore 67 @@ -153,8 +155,7 @@ namespace OpenSim.Region.Physics.Manager { get { return new NullPhysicsActor(); } } - - + public virtual bool Building { get; set; } public virtual ContactData ContactData diff --git a/OpenSim/Region/Physics/Manager/VehicleConstants.cs b/OpenSim/Region/Physics/Manager/VehicleConstants.cs index f0775c1..8e24b4c 100644 --- a/OpenSim/Region/Physics/Manager/VehicleConstants.cs +++ b/OpenSim/Region/Physics/Manager/VehicleConstants.cs @@ -26,6 +26,7 @@ */ using System; +using OpenMetaverse; namespace OpenSim.Region.Physics.Manager { @@ -117,5 +118,47 @@ namespace OpenSim.Region.Physics.Manager NO_DEFLECTION = 16392, LOCK_ROTATION = 32784 } - + + public struct VehicleData + { + public Vehicle m_type; + public VehicleFlag m_flags; + + // Linear properties + public Vector3 m_linearMotorDirection; + public Vector3 m_linearFrictionTimescale; + public float m_linearMotorDecayTimescale; + public float m_linearMotorTimescale; + public Vector3 m_linearMotorOffset; + + //Angular properties + public Vector3 m_angularMotorDirection; + public float m_angularMotorTimescale; + public float m_angularMotorDecayTimescale; + public Vector3 m_angularFrictionTimescale; + + //Deflection properties + public float m_angularDeflectionEfficiency; + public float m_angularDeflectionTimescale; + public float m_linearDeflectionEfficiency; + public float m_linearDeflectionTimescale; + + //Banking properties + public float m_bankingEfficiency; + public float m_bankingMix; + public float m_bankingTimescale; + + //Hover and Buoyancy properties + public float m_VhoverHeight; + public float m_VhoverEfficiency; + public float m_VhoverTimescale; + public float m_VehicleBuoyancy; + + //Attractor properties + public float m_verticalAttractionEfficiency; + public float m_verticalAttractionTimescale; + + // Axis + public Quaternion m_referenceFrame; + } } -- cgit v1.1 From 70b3b599bc09385c03cea0b0aca655f090b02e9f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 18:25:48 +0000 Subject: added some ToXml2 to SOGVehicle ( unused untested ) --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 85 ++++++++++++++++++++++++++- 1 file changed, 84 insertions(+), 1 deletion(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index 470f403..d06224c 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -29,7 +29,10 @@ using System; using OpenMetaverse; using OpenSim.Framework; using OpenSim.Region.Physics.Manager; - +using System.Xml; +using OpenSim.Framework.Serialization; +using OpenSim.Framework.Serialization.External; +using OpenSim.Region.Framework.Scenes.Serialization; namespace OpenSim.Region.Framework.Scenes { @@ -476,5 +479,85 @@ namespace OpenSim.Region.Framework.Scenes ph.VehicleFlags((int)m_flags, false); */ } + + private XmlTextWriter writer; + + private void XWint(string name, int i) + { + writer.WriteElementString(name, i.ToString()); + } + private void XWfloat(string name, float f) + { + writer.WriteElementString(name, f.ToString()); + } + + private void XWVector(string name, Vector3 vec) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", vec.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", vec.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", vec.Z.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + + + private void XWQuat(string name, Quaternion quat) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", quat.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", quat.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", quat.Z.ToString(Utils.EnUsCulture)); + writer.WriteElementString("W", quat.W.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + public void ToXml2(XmlTextWriter twriter) + { + writer = twriter; + writer.WriteStartElement("SceneObjectVehicle"); + + XWint("V_Type", (int)vd.m_type); + XWint("V_Flags", (int)vd.m_flags); + + // Linear properties + XWVector("LM_DIRECTION", vd.m_linearMotorDirection); + XWVector("LM_FRICTION_TIMESCALE", vd.m_linearFrictionTimescale); + XWfloat("LM_DECAY_TIMESCALE", vd.m_linearMotorDecayTimescale); + XWfloat("LM_TIMESCALE", vd.m_linearMotorTimescale); + XWVector("LM_OFFSET", vd.m_linearMotorOffset); + + //Angular properties + XWVector("AM_DIRECTION", vd.m_angularMotorDirection); + XWfloat("AM_TIMESCALE", vd.m_angularMotorTimescale); + XWfloat("AM_DECAY_TIMESCALE", vd.m_angularMotorDecayTimescale); + XWVector("AM_FRICTION_TIMESCALE", vd.m_angularFrictionTimescale); + + //Deflection properties + XWfloat("AD_EFFICIENCY", vd.m_angularDeflectionEfficiency); + XWfloat("AD_TIMESCALE", vd.m_angularDeflectionTimescale); + XWfloat("LD_EFFICIENCY", vd.m_linearDeflectionEfficiency); + XWfloat("LD_TIMESCALE", vd.m_linearDeflectionTimescale); + + //Banking properties + XWfloat("B_EFFICIENCY", vd.m_bankingEfficiency); + XWfloat("B_MIX", vd.m_bankingMix); + XWfloat("B_TIMESCALE", vd.m_bankingTimescale); + + //Hover and Buoyancy properties + XWfloat("H_HEIGHT", vd.m_VhoverHeight); + XWfloat("H_EFFICIENCY", vd.m_VhoverEfficiency); + XWfloat("H_TIMESCALE", vd.m_VhoverTimescale); + XWfloat("VBUOYANCY", vd.m_VehicleBuoyancy); + + //Attractor properties + XWfloat("VA_EFFICIENCY", vd.m_verticalAttractionEfficiency); + XWfloat("VA_TIMESCALE", vd.m_verticalAttractionTimescale); + + XWQuat("REF_FRAME", vd.m_referenceFrame); + + writer.WriteEndElement(); + writer = null; + } } } \ No newline at end of file -- cgit v1.1 From 028dc1f4ed2e8bdc5e3d5e3d493a743e6d7b557f Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 18:33:41 +0000 Subject: a bit less human readable xml --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 50 +++++++++++++-------------- 1 file changed, 25 insertions(+), 25 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index d06224c..5a875ee 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -515,44 +515,44 @@ namespace OpenSim.Region.Framework.Scenes public void ToXml2(XmlTextWriter twriter) { writer = twriter; - writer.WriteStartElement("SceneObjectVehicle"); + writer.WriteStartElement("SOGVehicle"); - XWint("V_Type", (int)vd.m_type); - XWint("V_Flags", (int)vd.m_flags); + XWint("TYPE", (int)vd.m_type); + XWint("FLAGS", (int)vd.m_flags); // Linear properties - XWVector("LM_DIRECTION", vd.m_linearMotorDirection); - XWVector("LM_FRICTION_TIMESCALE", vd.m_linearFrictionTimescale); - XWfloat("LM_DECAY_TIMESCALE", vd.m_linearMotorDecayTimescale); - XWfloat("LM_TIMESCALE", vd.m_linearMotorTimescale); - XWVector("LM_OFFSET", vd.m_linearMotorOffset); + XWVector("LMDIR", vd.m_linearMotorDirection); + XWVector("LMFTIME", vd.m_linearFrictionTimescale); + XWfloat("LMDTIME", vd.m_linearMotorDecayTimescale); + XWfloat("LMTIME", vd.m_linearMotorTimescale); + XWVector("LMOFF", vd.m_linearMotorOffset); //Angular properties - XWVector("AM_DIRECTION", vd.m_angularMotorDirection); - XWfloat("AM_TIMESCALE", vd.m_angularMotorTimescale); - XWfloat("AM_DECAY_TIMESCALE", vd.m_angularMotorDecayTimescale); - XWVector("AM_FRICTION_TIMESCALE", vd.m_angularFrictionTimescale); + XWVector("AMDIR", vd.m_angularMotorDirection); + XWfloat("AMTIME", vd.m_angularMotorTimescale); + XWfloat("AMDTIME", vd.m_angularMotorDecayTimescale); + XWVector("AMFTIME", vd.m_angularFrictionTimescale); //Deflection properties - XWfloat("AD_EFFICIENCY", vd.m_angularDeflectionEfficiency); - XWfloat("AD_TIMESCALE", vd.m_angularDeflectionTimescale); - XWfloat("LD_EFFICIENCY", vd.m_linearDeflectionEfficiency); - XWfloat("LD_TIMESCALE", vd.m_linearDeflectionTimescale); + XWfloat("ADEFF", vd.m_angularDeflectionEfficiency); + XWfloat("ADTIME", vd.m_angularDeflectionTimescale); + XWfloat("LDEFF", vd.m_linearDeflectionEfficiency); + XWfloat("LDTIME", vd.m_linearDeflectionTimescale); //Banking properties - XWfloat("B_EFFICIENCY", vd.m_bankingEfficiency); - XWfloat("B_MIX", vd.m_bankingMix); - XWfloat("B_TIMESCALE", vd.m_bankingTimescale); + XWfloat("BEFF", vd.m_bankingEfficiency); + XWfloat("BMIX", vd.m_bankingMix); + XWfloat("BTIME", vd.m_bankingTimescale); //Hover and Buoyancy properties - XWfloat("H_HEIGHT", vd.m_VhoverHeight); - XWfloat("H_EFFICIENCY", vd.m_VhoverEfficiency); - XWfloat("H_TIMESCALE", vd.m_VhoverTimescale); - XWfloat("VBUOYANCY", vd.m_VehicleBuoyancy); + XWfloat("HHEI", vd.m_VhoverHeight); + XWfloat("HEFF", vd.m_VhoverEfficiency); + XWfloat("HTIME", vd.m_VhoverTimescale); + XWfloat("VBUO", vd.m_VehicleBuoyancy); //Attractor properties - XWfloat("VA_EFFICIENCY", vd.m_verticalAttractionEfficiency); - XWfloat("VA_TIMESCALE", vd.m_verticalAttractionTimescale); + XWfloat("VAEFF", vd.m_verticalAttractionEfficiency); + XWfloat("VATIME", vd.m_verticalAttractionTimescale); XWQuat("REF_FRAME", vd.m_referenceFrame); -- cgit v1.1 From a62baf8ea453c7798057895d7c09ac0b81116f27 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 18:37:29 +0000 Subject: added a EnUsCulture whatever that is.. --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index 5a875ee..ac70d04 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -484,11 +484,12 @@ namespace OpenSim.Region.Framework.Scenes private void XWint(string name, int i) { - writer.WriteElementString(name, i.ToString()); + writer.WriteElementString(name, i.ToString()); } + private void XWfloat(string name, float f) { - writer.WriteElementString(name, f.ToString()); + writer.WriteElementString(name, f.ToString(Utils.EnUsCulture)); } private void XWVector(string name, Vector3 vec) @@ -500,8 +501,6 @@ namespace OpenSim.Region.Framework.Scenes writer.WriteEndElement(); } - - private void XWQuat(string name, Quaternion quat) { writer.WriteStartElement(name); -- cgit v1.1 From 7a1ab03b7534fdd76bb0266216e3a5990b93ee77 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sat, 18 Feb 2012 19:31:36 +0000 Subject: missing paramenters init plus some cleaning --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 107 ++++---------------------- 1 file changed, 16 insertions(+), 91 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs index ac70d04..5651de3 100644 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs @@ -38,53 +38,13 @@ namespace OpenSim.Region.Framework.Scenes { public class SOGVehicle { + public VehicleData vd; + public Vehicle Type { get { return vd.m_type; } } - - public VehicleData vd; -/* - private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind - private VehicleFlag m_flags = (VehicleFlag)0; - - // Linear properties - private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time - private Vector3 m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); - private float m_linearMotorDecayTimescale = 120; - private float m_linearMotorTimescale = 1000; - private Vector3 m_linearMotorOffset = Vector3.Zero; - - //Angular properties - private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor - private float m_angularMotorTimescale = 1000; // motor angular velocity ramp up rate - private float m_angularMotorDecayTimescale = 120; // motor angular velocity decay rate - private Vector3 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); // body angular velocity decay rate - - //Deflection properties - private float m_angularDeflectionEfficiency = 0; - private float m_angularDeflectionTimescale = 1000; - private float m_linearDeflectionEfficiency = 0; - private float m_linearDeflectionTimescale = 1000; - - //Banking properties - private float m_bankingEfficiency = 0; - private float m_bankingMix = 0; - private float m_bankingTimescale = 0; - - //Hover and Buoyancy properties - private float m_VhoverHeight = 0f; - private float m_VhoverEfficiency = 0f; - private float m_VhoverTimescale = 1000f; - private float m_VehicleBuoyancy = 0f; - - //Attractor properties - private float m_verticalAttractionEfficiency = 1.0f; // damped - private float m_verticalAttractionTimescale = 1000f; // Timescale > 300 means no vert attractor. - - // Axis - public Quaternion m_referenceFrame = Quaternion.Identity; -*/ + public SOGVehicle() { vd = new VehicleData(); @@ -274,16 +234,14 @@ namespace OpenSim.Region.Framework.Scenes { vd.m_linearMotorDirection = Vector3.Zero; vd.m_angularMotorDirection = Vector3.Zero; - vd.m_linearMotorOffset = Vector3.Zero; - vd.m_referenceFrame = Quaternion.Identity; // Set Defaults For Type vd.m_type = pType; switch (pType) { - case Vehicle.TYPE_NONE: // none sense this will never exist + case Vehicle.TYPE_NONE: vd.m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); vd.m_linearMotorTimescale = 1000; @@ -291,8 +249,19 @@ namespace OpenSim.Region.Framework.Scenes vd.m_angularMotorTimescale = 1000; vd.m_angularMotorDecayTimescale = 1000; vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; vd.m_VhoverTimescale = 1000; vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 1000; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1000; + vd.m_verticalAttractionEfficiency = 0; + vd.m_verticalAttractionTimescale = 1000; + vd.m_flags = (VehicleFlag)0; break; @@ -431,53 +400,9 @@ namespace OpenSim.Region.Framework.Scenes } public void SetVehicle(PhysicsActor ph) { - - // crap crap crap - if (ph == null) // what ?? + if (ph == null) return; ph.SetVehicle(vd); -/* - ph.VehicleType = (int)m_type; - - // Linear properties - ph.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_DIRECTION, m_linearMotorDirection); - ph.VehicleVectorParam((int)Vehicle.LINEAR_FRICTION_TIMESCALE, m_linearFrictionTimescale); - ph.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE, m_linearMotorDecayTimescale); - ph.VehicleFloatParam((int)Vehicle.LINEAR_MOTOR_TIMESCALE, m_linearMotorTimescale); - ph.VehicleVectorParam((int)Vehicle.LINEAR_MOTOR_OFFSET, m_linearMotorOffset); - - //Angular properties - ph.VehicleVectorParam((int)Vehicle.ANGULAR_MOTOR_DIRECTION, m_angularMotorDirection); - ph.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_TIMESCALE, m_angularMotorTimescale); - ph.VehicleFloatParam((int)Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE, m_angularMotorDecayTimescale); - ph.VehicleVectorParam((int)Vehicle.ANGULAR_FRICTION_TIMESCALE, m_angularFrictionTimescale); - - //Deflection properties - ph.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_EFFICIENCY, m_angularDeflectionEfficiency); - ph.VehicleFloatParam((int)Vehicle.ANGULAR_DEFLECTION_TIMESCALE, m_angularDeflectionTimescale); - ph.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_EFFICIENCY, m_linearDeflectionEfficiency); - ph.VehicleFloatParam((int)Vehicle.LINEAR_DEFLECTION_TIMESCALE, m_linearDeflectionTimescale); - - //Banking properties - ph.VehicleFloatParam((int)Vehicle.BANKING_EFFICIENCY, m_bankingEfficiency); - ph.VehicleFloatParam((int)Vehicle.BANKING_MIX, m_bankingMix); - ph.VehicleFloatParam((int)Vehicle.BANKING_TIMESCALE, m_bankingTimescale); - - //Hover and Buoyancy properties - ph.VehicleFloatParam((int)Vehicle.HOVER_HEIGHT, m_VhoverHeight); - ph.VehicleFloatParam((int)Vehicle.HOVER_EFFICIENCY, m_VhoverEfficiency); - ph.VehicleFloatParam((int)Vehicle.HOVER_TIMESCALE, m_VhoverTimescale); - ph.VehicleFloatParam((int)Vehicle.BUOYANCY, m_VehicleBuoyancy); - - //Attractor properties - ph.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_EFFICIENCY, m_verticalAttractionEfficiency); - ph.VehicleFloatParam((int)Vehicle.VERTICAL_ATTRACTION_TIMESCALE, m_verticalAttractionTimescale); - - ph.VehicleRotationParam((int)Vehicle.REFERENCE_FRAME, m_referenceFrame); - - ph.VehicleFlags(~(int)m_flags, true); - ph.VehicleFlags((int)m_flags, false); - */ } private XmlTextWriter writer; -- cgit v1.1 From 736fb0b41dae9ab4a66e556bbb37aeb9d6360f1d Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 19 Feb 2012 02:07:43 +0000 Subject: minor fix to chODE terrain heighmap scale --- OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'OpenSim') diff --git a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs index 03059f7..b5b30ee 100644 --- a/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs +++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs @@ -3440,15 +3440,15 @@ namespace OpenSim.Region.Physics.OdePlugin int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M) - int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra + int heightmapWidth = regionsize + 2; // ODE map size 258 x 258 (Meters) (1 extra each side) int heightmapHeight = regionsize + 2; - int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples - int heightmapHeightSamples = (int)regionsize + 2; + int heightmapWidthSamples = (int)regionsize + 3; // to have 258m we need 259 samples + int heightmapHeightSamples = (int)regionsize + 3; // Array of height samples for ODE float[] _heightmap; - _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258 + _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 259 x 259 // Other ODE parameters const float scale = 1.0f; -- cgit v1.1 From b77d354e6dcf1eb31486f0db3236780f63f23844 Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 19 Feb 2012 13:21:01 +0000 Subject: moved vehicle from SOG to SOP --- OpenSim/Region/Framework/Scenes/SOGVehicle.cs | 487 --------------------- OpenSim/Region/Framework/Scenes/SOPVehicle.cs | 487 +++++++++++++++++++++ .../Region/Framework/Scenes/SceneObjectGroup.cs | 90 ---- OpenSim/Region/Framework/Scenes/SceneObjectPart.cs | 87 +++- 4 files changed, 563 insertions(+), 588 deletions(-) delete mode 100644 OpenSim/Region/Framework/Scenes/SOGVehicle.cs create mode 100644 OpenSim/Region/Framework/Scenes/SOPVehicle.cs (limited to 'OpenSim') diff --git a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs b/OpenSim/Region/Framework/Scenes/SOGVehicle.cs deleted file mode 100644 index 5651de3..0000000 --- a/OpenSim/Region/Framework/Scenes/SOGVehicle.cs +++ /dev/null @@ -1,487 +0,0 @@ -/* - * Copyright (c) Contributors, http://opensimulator.org/ - * See CONTRIBUTORS.TXT for a full list of copyright holders. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the OpenSimulator Project nor the - * names of its contributors may be used to endorse or promote products - * derived from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY - * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY - * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES - * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND - * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT - * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS - * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -using System; -using OpenMetaverse; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; -using System.Xml; -using OpenSim.Framework.Serialization; -using OpenSim.Framework.Serialization.External; -using OpenSim.Region.Framework.Scenes.Serialization; - -namespace OpenSim.Region.Framework.Scenes -{ - public class SOGVehicle - { - public VehicleData vd; - - public Vehicle Type - { - get { return vd.m_type; } - } - - public SOGVehicle() - { - vd = new VehicleData(); - ProcessTypeChange(Vehicle.TYPE_NONE); // is needed? - } - - public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) - { - float len; - float timestep = 0.01f; - switch (pParam) - { - case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - vd.m_angularDeflectionEfficiency = pValue; - break; - case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_angularDeflectionTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: - if (pValue < timestep) pValue = timestep; - else if (pValue > 120) pValue = 120; - vd.m_angularMotorDecayTimescale = pValue; - break; - case Vehicle.ANGULAR_MOTOR_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_angularMotorTimescale = pValue; - break; - case Vehicle.BANKING_EFFICIENCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - vd.m_bankingEfficiency = pValue; - break; - case Vehicle.BANKING_MIX: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - vd.m_bankingMix = pValue; - break; - case Vehicle.BANKING_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_bankingTimescale = pValue; - break; - case Vehicle.BUOYANCY: - if (pValue < -1f) pValue = -1f; - if (pValue > 1f) pValue = 1f; - vd.m_VehicleBuoyancy = pValue; - break; - case Vehicle.HOVER_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - vd.m_VhoverEfficiency = pValue; - break; - case Vehicle.HOVER_HEIGHT: - vd.m_VhoverHeight = pValue; - break; - case Vehicle.HOVER_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_VhoverTimescale = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - vd.m_linearDeflectionEfficiency = pValue; - break; - case Vehicle.LINEAR_DEFLECTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_linearDeflectionTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: - // if (pValue < timestep) pValue = timestep; - // try to make impulses to work a bit better - if (pValue < timestep) pValue = timestep; - else if (pValue > 120) pValue = 120; - vd.m_linearMotorDecayTimescale = pValue; - break; - case Vehicle.LINEAR_MOTOR_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_linearMotorTimescale = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: - if (pValue < 0f) pValue = 0f; - if (pValue > 1f) pValue = 1f; - vd.m_verticalAttractionEfficiency = pValue; - break; - case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_verticalAttractionTimescale = pValue; - break; - - // These are vector properties but the engine lets you use a single float value to - // set all of the components to the same value - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - vd.m_angularMotorDirection = new Vector3(pValue, pValue, pValue); - len = vd.m_angularMotorDirection.Length(); - if (len > 12.566f) - vd.m_angularMotorDirection *= (12.566f / len); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue < timestep) pValue = timestep; - vd.m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - vd.m_linearMotorDirection = new Vector3(pValue, pValue, pValue); - len = vd.m_linearMotorDirection.Length(); - if (len > 30.0f) - vd.m_linearMotorDirection *= (30.0f / len); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - vd.m_linearMotorOffset = new Vector3(pValue, pValue, pValue); - len = vd.m_linearMotorOffset.Length(); - if (len > 100.0f) - vd.m_linearMotorOffset *= (100.0f / len); - break; - } - }//end ProcessFloatVehicleParam - - public void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) - { - float len; - float timestep = 0.01f; - switch (pParam) - { - case Vehicle.ANGULAR_FRICTION_TIMESCALE: - if (pValue.X < timestep) pValue.X = timestep; - if (pValue.Y < timestep) pValue.Y = timestep; - if (pValue.Z < timestep) pValue.Z = timestep; - - vd.m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.ANGULAR_MOTOR_DIRECTION: - vd.m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - // Limit requested angular speed to 2 rps= 4 pi rads/sec - len = vd.m_angularMotorDirection.Length(); - if (len > 12.566f) - vd.m_angularMotorDirection *= (12.566f / len); - break; - case Vehicle.LINEAR_FRICTION_TIMESCALE: - if (pValue.X < timestep) pValue.X = timestep; - if (pValue.Y < timestep) pValue.Y = timestep; - if (pValue.Z < timestep) pValue.Z = timestep; - vd.m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); - break; - case Vehicle.LINEAR_MOTOR_DIRECTION: - vd.m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - len = vd.m_linearMotorDirection.Length(); - if (len > 30.0f) - vd.m_linearMotorDirection *= (30.0f / len); - break; - case Vehicle.LINEAR_MOTOR_OFFSET: - vd.m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); - len = vd.m_linearMotorOffset.Length(); - if (len > 100.0f) - vd.m_linearMotorOffset *= (100.0f / len); - break; - } - }//end ProcessVectorVehicleParam - - public void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) - { - switch (pParam) - { - case Vehicle.REFERENCE_FRAME: - vd.m_referenceFrame = Quaternion.Inverse(pValue); - break; - } - }//end ProcessRotationVehicleParam - - public void ProcessVehicleFlags(int pParam, bool remove) - { - if (remove) - { - vd.m_flags &= ~((VehicleFlag)pParam); - } - else - { - vd.m_flags |= (VehicleFlag)pParam; - } - }//end ProcessVehicleFlags - - public void ProcessTypeChange(Vehicle pType) - { - vd.m_linearMotorDirection = Vector3.Zero; - vd.m_angularMotorDirection = Vector3.Zero; - vd.m_linearMotorOffset = Vector3.Zero; - vd.m_referenceFrame = Quaternion.Identity; - - // Set Defaults For Type - vd.m_type = pType; - switch (pType) - { - case Vehicle.TYPE_NONE: - vd.m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); - vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - vd.m_linearMotorTimescale = 1000; - vd.m_linearMotorDecayTimescale = 120; - vd.m_angularMotorTimescale = 1000; - vd.m_angularMotorDecayTimescale = 1000; - vd.m_VhoverHeight = 0; - vd.m_VhoverEfficiency = 1; - vd.m_VhoverTimescale = 1000; - vd.m_VehicleBuoyancy = 0; - vd.m_linearDeflectionEfficiency = 0; - vd.m_linearDeflectionTimescale = 1000; - vd.m_angularDeflectionEfficiency = 0; - vd.m_angularDeflectionTimescale = 1000; - vd.m_bankingEfficiency = 0; - vd.m_bankingMix = 1; - vd.m_bankingTimescale = 1000; - vd.m_verticalAttractionEfficiency = 0; - vd.m_verticalAttractionTimescale = 1000; - - vd.m_flags = (VehicleFlag)0; - break; - - case Vehicle.TYPE_SLED: - vd.m_linearFrictionTimescale = new Vector3(30, 1, 1000); - vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - vd.m_linearMotorTimescale = 1000; - vd.m_linearMotorDecayTimescale = 120; - vd.m_angularMotorTimescale = 1000; - vd.m_angularMotorDecayTimescale = 120; - vd.m_VhoverHeight = 0; - vd.m_VhoverEfficiency = 1; - vd.m_VhoverTimescale = 10; - vd.m_VehicleBuoyancy = 0; - vd.m_linearDeflectionEfficiency = 1; - vd.m_linearDeflectionTimescale = 1; - vd.m_angularDeflectionEfficiency = 0; - vd.m_angularDeflectionTimescale = 1000; - vd.m_bankingEfficiency = 0; - vd.m_bankingMix = 1; - vd.m_bankingTimescale = 10; - vd.m_flags &= - ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); - vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); - break; - case Vehicle.TYPE_CAR: - vd.m_linearFrictionTimescale = new Vector3(100, 2, 1000); - vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); - vd.m_linearMotorTimescale = 1; - vd.m_linearMotorDecayTimescale = 60; - vd.m_angularMotorTimescale = 1; - vd.m_angularMotorDecayTimescale = 0.8f; - vd.m_VhoverHeight = 0; - vd.m_VhoverEfficiency = 0; - vd.m_VhoverTimescale = 1000; - vd.m_VehicleBuoyancy = 0; - vd.m_linearDeflectionEfficiency = 1; - vd.m_linearDeflectionTimescale = 2; - vd.m_angularDeflectionEfficiency = 0; - vd.m_angularDeflectionTimescale = 10; - vd.m_verticalAttractionEfficiency = 1f; - vd.m_verticalAttractionTimescale = 10f; - vd.m_bankingEfficiency = -0.2f; - vd.m_bankingMix = 1; - vd.m_bankingTimescale = 1; - vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); - vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); - break; - case Vehicle.TYPE_BOAT: - vd.m_linearFrictionTimescale = new Vector3(10, 3, 2); - vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); - vd.m_linearMotorTimescale = 5; - vd.m_linearMotorDecayTimescale = 60; - vd.m_angularMotorTimescale = 4; - vd.m_angularMotorDecayTimescale = 4; - vd.m_VhoverHeight = 0; - vd.m_VhoverEfficiency = 0.5f; - vd.m_VhoverTimescale = 2; - vd.m_VehicleBuoyancy = 1; - vd.m_linearDeflectionEfficiency = 0.5f; - vd.m_linearDeflectionTimescale = 3; - vd.m_angularDeflectionEfficiency = 0.5f; - vd.m_angularDeflectionTimescale = 5; - vd.m_verticalAttractionEfficiency = 0.5f; - vd.m_verticalAttractionTimescale = 5f; - vd.m_bankingEfficiency = -0.3f; - vd.m_bankingMix = 0.8f; - vd.m_bankingTimescale = 1; - vd.m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.LIMIT_ROLL_ONLY); - vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP | - VehicleFlag.HOVER_WATER_ONLY); - break; - case Vehicle.TYPE_AIRPLANE: - vd.m_linearFrictionTimescale = new Vector3(200, 10, 5); - vd.m_angularFrictionTimescale = new Vector3(20, 20, 20); - vd.m_linearMotorTimescale = 2; - vd.m_linearMotorDecayTimescale = 60; - vd.m_angularMotorTimescale = 4; - vd.m_angularMotorDecayTimescale = 8; - vd.m_VhoverHeight = 0; - vd.m_VhoverEfficiency = 0.5f; - vd.m_VhoverTimescale = 1000; - vd.m_VehicleBuoyancy = 0; - vd.m_linearDeflectionEfficiency = 0.5f; - vd.m_linearDeflectionTimescale = 0.5f; - vd.m_angularDeflectionEfficiency = 1; - vd.m_angularDeflectionTimescale = 2; - vd.m_verticalAttractionEfficiency = 0.9f; - vd.m_verticalAttractionTimescale = 2f; - vd.m_bankingEfficiency = 1; - vd.m_bankingMix = 0.7f; - vd.m_bankingTimescale = 2; - vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP); - vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); - break; - case Vehicle.TYPE_BALLOON: - vd.m_linearFrictionTimescale = new Vector3(5, 5, 5); - vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); - vd.m_linearMotorTimescale = 5; - vd.m_linearMotorDecayTimescale = 60; - vd.m_angularMotorTimescale = 6; - vd.m_angularMotorDecayTimescale = 10; - vd.m_VhoverHeight = 5; - vd.m_VhoverEfficiency = 0.8f; - vd.m_VhoverTimescale = 10; - vd.m_VehicleBuoyancy = 1; - vd.m_linearDeflectionEfficiency = 0; - vd.m_linearDeflectionTimescale = 5; - vd.m_angularDeflectionEfficiency = 0; - vd.m_angularDeflectionTimescale = 5; - vd.m_verticalAttractionEfficiency = 0f; - vd.m_verticalAttractionTimescale = 1000f; - vd.m_bankingEfficiency = 0; - vd.m_bankingMix = 0.7f; - vd.m_bankingTimescale = 5; - vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | - VehicleFlag.HOVER_TERRAIN_ONLY | - VehicleFlag.HOVER_UP_ONLY | - VehicleFlag.NO_DEFLECTION_UP | - VehicleFlag.LIMIT_MOTOR_UP); - vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | - VehicleFlag.HOVER_GLOBAL_HEIGHT); - break; - } - } - public void SetVehicle(PhysicsActor ph) - { - if (ph == null) - return; - ph.SetVehicle(vd); - } - - private XmlTextWriter writer; - - private void XWint(string name, int i) - { - writer.WriteElementString(name, i.ToString()); - } - - private void XWfloat(string name, float f) - { - writer.WriteElementString(name, f.ToString(Utils.EnUsCulture)); - } - - private void XWVector(string name, Vector3 vec) - { - writer.WriteStartElement(name); - writer.WriteElementString("X", vec.X.ToString(Utils.EnUsCulture)); - writer.WriteElementString("Y", vec.Y.ToString(Utils.EnUsCulture)); - writer.WriteElementString("Z", vec.Z.ToString(Utils.EnUsCulture)); - writer.WriteEndElement(); - } - - private void XWQuat(string name, Quaternion quat) - { - writer.WriteStartElement(name); - writer.WriteElementString("X", quat.X.ToString(Utils.EnUsCulture)); - writer.WriteElementString("Y", quat.Y.ToString(Utils.EnUsCulture)); - writer.WriteElementString("Z", quat.Z.ToString(Utils.EnUsCulture)); - writer.WriteElementString("W", quat.W.ToString(Utils.EnUsCulture)); - writer.WriteEndElement(); - } - - public void ToXml2(XmlTextWriter twriter) - { - writer = twriter; - writer.WriteStartElement("SOGVehicle"); - - XWint("TYPE", (int)vd.m_type); - XWint("FLAGS", (int)vd.m_flags); - - // Linear properties - XWVector("LMDIR", vd.m_linearMotorDirection); - XWVector("LMFTIME", vd.m_linearFrictionTimescale); - XWfloat("LMDTIME", vd.m_linearMotorDecayTimescale); - XWfloat("LMTIME", vd.m_linearMotorTimescale); - XWVector("LMOFF", vd.m_linearMotorOffset); - - //Angular properties - XWVector("AMDIR", vd.m_angularMotorDirection); - XWfloat("AMTIME", vd.m_angularMotorTimescale); - XWfloat("AMDTIME", vd.m_angularMotorDecayTimescale); - XWVector("AMFTIME", vd.m_angularFrictionTimescale); - - //Deflection properties - XWfloat("ADEFF", vd.m_angularDeflectionEfficiency); - XWfloat("ADTIME", vd.m_angularDeflectionTimescale); - XWfloat("LDEFF", vd.m_linearDeflectionEfficiency); - XWfloat("LDTIME", vd.m_linearDeflectionTimescale); - - //Banking properties - XWfloat("BEFF", vd.m_bankingEfficiency); - XWfloat("BMIX", vd.m_bankingMix); - XWfloat("BTIME", vd.m_bankingTimescale); - - //Hover and Buoyancy properties - XWfloat("HHEI", vd.m_VhoverHeight); - XWfloat("HEFF", vd.m_VhoverEfficiency); - XWfloat("HTIME", vd.m_VhoverTimescale); - XWfloat("VBUO", vd.m_VehicleBuoyancy); - - //Attractor properties - XWfloat("VAEFF", vd.m_verticalAttractionEfficiency); - XWfloat("VATIME", vd.m_verticalAttractionTimescale); - - XWQuat("REF_FRAME", vd.m_referenceFrame); - - writer.WriteEndElement(); - writer = null; - } - } -} \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/SOPVehicle.cs b/OpenSim/Region/Framework/Scenes/SOPVehicle.cs new file mode 100644 index 0000000..39786bd --- /dev/null +++ b/OpenSim/Region/Framework/Scenes/SOPVehicle.cs @@ -0,0 +1,487 @@ +/* + * Copyright (c) Contributors, http://opensimulator.org/ + * See CONTRIBUTORS.TXT for a full list of copyright holders. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +using System; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using System.Xml; +using OpenSim.Framework.Serialization; +using OpenSim.Framework.Serialization.External; +using OpenSim.Region.Framework.Scenes.Serialization; + +namespace OpenSim.Region.Framework.Scenes +{ + public class SOPVehicle + { + public VehicleData vd; + + public Vehicle Type + { + get { return vd.m_type; } + } + + public SOPVehicle() + { + vd = new VehicleData(); + ProcessTypeChange(Vehicle.TYPE_NONE); // is needed? + } + + public void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + vd.m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + vd.m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + vd.m_VehicleBuoyancy = pValue; + break; + case Vehicle.HOVER_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_VhoverEfficiency = pValue; + break; + case Vehicle.HOVER_HEIGHT: + vd.m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + // if (pValue < timestep) pValue = timestep; + // try to make impulses to work a bit better + if (pValue < timestep) pValue = timestep; + else if (pValue > 120) pValue = 120; + vd.m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0f) pValue = 0f; + if (pValue > 1f) pValue = 1f; + vd.m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + vd.m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_angularMotorDirection.Length(); + if (len > 12.566f) + vd.m_angularMotorDirection *= (12.566f / len); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue < timestep) pValue = timestep; + vd.m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + vd.m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorDirection.Length(); + if (len > 30.0f) + vd.m_linearMotorDirection *= (30.0f / len); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + vd.m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + len = vd.m_linearMotorOffset.Length(); + if (len > 100.0f) + vd.m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessFloatVehicleParam + + public void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + float len; + float timestep = 0.01f; + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + + vd.m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + vd.m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + len = vd.m_angularMotorDirection.Length(); + if (len > 12.566f) + vd.m_angularMotorDirection *= (12.566f / len); + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + if (pValue.X < timestep) pValue.X = timestep; + if (pValue.Y < timestep) pValue.Y = timestep; + if (pValue.Z < timestep) pValue.Z = timestep; + vd.m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + vd.m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorDirection.Length(); + if (len > 30.0f) + vd.m_linearMotorDirection *= (30.0f / len); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + vd.m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + len = vd.m_linearMotorOffset.Length(); + if (len > 100.0f) + vd.m_linearMotorOffset *= (100.0f / len); + break; + } + }//end ProcessVectorVehicleParam + + public void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + vd.m_referenceFrame = Quaternion.Inverse(pValue); + break; + } + }//end ProcessRotationVehicleParam + + public void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + vd.m_flags &= ~((VehicleFlag)pParam); + } + else + { + vd.m_flags |= (VehicleFlag)pParam; + } + }//end ProcessVehicleFlags + + public void ProcessTypeChange(Vehicle pType) + { + vd.m_linearMotorDirection = Vector3.Zero; + vd.m_angularMotorDirection = Vector3.Zero; + vd.m_linearMotorOffset = Vector3.Zero; + vd.m_referenceFrame = Quaternion.Identity; + + // Set Defaults For Type + vd.m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + vd.m_linearFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 1000; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 1000; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1000; + vd.m_verticalAttractionEfficiency = 0; + vd.m_verticalAttractionTimescale = 1000; + + vd.m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + vd.m_linearFrictionTimescale = new Vector3(30, 1, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1000; + vd.m_linearMotorDecayTimescale = 120; + vd.m_angularMotorTimescale = 1000; + vd.m_angularMotorDecayTimescale = 120; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 1; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 1; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 1000; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 10; + vd.m_flags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + vd.m_linearFrictionTimescale = new Vector3(100, 2, 1000); + vd.m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + vd.m_linearMotorTimescale = 1; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 1; + vd.m_angularMotorDecayTimescale = 0.8f; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 1; + vd.m_linearDeflectionTimescale = 2; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 10; + vd.m_verticalAttractionEfficiency = 1f; + vd.m_verticalAttractionTimescale = 10f; + vd.m_bankingEfficiency = -0.2f; + vd.m_bankingMix = 1; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP | VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + vd.m_linearFrictionTimescale = new Vector3(10, 3, 2); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 4; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 2; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 3; + vd.m_angularDeflectionEfficiency = 0.5f; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0.5f; + vd.m_verticalAttractionTimescale = 5f; + vd.m_bankingEfficiency = -0.3f; + vd.m_bankingMix = 0.8f; + vd.m_bankingTimescale = 1; + vd.m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.LIMIT_ROLL_ONLY); + vd.m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP | + VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + vd.m_linearFrictionTimescale = new Vector3(200, 10, 5); + vd.m_angularFrictionTimescale = new Vector3(20, 20, 20); + vd.m_linearMotorTimescale = 2; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 4; + vd.m_angularMotorDecayTimescale = 8; + vd.m_VhoverHeight = 0; + vd.m_VhoverEfficiency = 0.5f; + vd.m_VhoverTimescale = 1000; + vd.m_VehicleBuoyancy = 0; + vd.m_linearDeflectionEfficiency = 0.5f; + vd.m_linearDeflectionTimescale = 0.5f; + vd.m_angularDeflectionEfficiency = 1; + vd.m_angularDeflectionTimescale = 2; + vd.m_verticalAttractionEfficiency = 0.9f; + vd.m_verticalAttractionTimescale = 2f; + vd.m_bankingEfficiency = 1; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 2; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + vd.m_linearFrictionTimescale = new Vector3(5, 5, 5); + vd.m_angularFrictionTimescale = new Vector3(10, 10, 10); + vd.m_linearMotorTimescale = 5; + vd.m_linearMotorDecayTimescale = 60; + vd.m_angularMotorTimescale = 6; + vd.m_angularMotorDecayTimescale = 10; + vd.m_VhoverHeight = 5; + vd.m_VhoverEfficiency = 0.8f; + vd.m_VhoverTimescale = 10; + vd.m_VehicleBuoyancy = 1; + vd.m_linearDeflectionEfficiency = 0; + vd.m_linearDeflectionTimescale = 5; + vd.m_angularDeflectionEfficiency = 0; + vd.m_angularDeflectionTimescale = 5; + vd.m_verticalAttractionEfficiency = 0f; + vd.m_verticalAttractionTimescale = 1000f; + vd.m_bankingEfficiency = 0; + vd.m_bankingMix = 0.7f; + vd.m_bankingTimescale = 5; + vd.m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | + VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY | + VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + vd.m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + } + } + public void SetVehicle(PhysicsActor ph) + { + if (ph == null) + return; + ph.SetVehicle(vd); + } + + private XmlTextWriter writer; + + private void XWint(string name, int i) + { + writer.WriteElementString(name, i.ToString()); + } + + private void XWfloat(string name, float f) + { + writer.WriteElementString(name, f.ToString(Utils.EnUsCulture)); + } + + private void XWVector(string name, Vector3 vec) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", vec.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", vec.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", vec.Z.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + private void XWQuat(string name, Quaternion quat) + { + writer.WriteStartElement(name); + writer.WriteElementString("X", quat.X.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Y", quat.Y.ToString(Utils.EnUsCulture)); + writer.WriteElementString("Z", quat.Z.ToString(Utils.EnUsCulture)); + writer.WriteElementString("W", quat.W.ToString(Utils.EnUsCulture)); + writer.WriteEndElement(); + } + + public void ToXml2(XmlTextWriter twriter) + { + writer = twriter; + writer.WriteStartElement("SOGVehicle"); + + XWint("TYPE", (int)vd.m_type); + XWint("FLAGS", (int)vd.m_flags); + + // Linear properties + XWVector("LMDIR", vd.m_linearMotorDirection); + XWVector("LMFTIME", vd.m_linearFrictionTimescale); + XWfloat("LMDTIME", vd.m_linearMotorDecayTimescale); + XWfloat("LMTIME", vd.m_linearMotorTimescale); + XWVector("LMOFF", vd.m_linearMotorOffset); + + //Angular properties + XWVector("AMDIR", vd.m_angularMotorDirection); + XWfloat("AMTIME", vd.m_angularMotorTimescale); + XWfloat("AMDTIME", vd.m_angularMotorDecayTimescale); + XWVector("AMFTIME", vd.m_angularFrictionTimescale); + + //Deflection properties + XWfloat("ADEFF", vd.m_angularDeflectionEfficiency); + XWfloat("ADTIME", vd.m_angularDeflectionTimescale); + XWfloat("LDEFF", vd.m_linearDeflectionEfficiency); + XWfloat("LDTIME", vd.m_linearDeflectionTimescale); + + //Banking properties + XWfloat("BEFF", vd.m_bankingEfficiency); + XWfloat("BMIX", vd.m_bankingMix); + XWfloat("BTIME", vd.m_bankingTimescale); + + //Hover and Buoyancy properties + XWfloat("HHEI", vd.m_VhoverHeight); + XWfloat("HEFF", vd.m_VhoverEfficiency); + XWfloat("HTIME", vd.m_VhoverTimescale); + XWfloat("VBUO", vd.m_VehicleBuoyancy); + + //Attractor properties + XWfloat("VAEFF", vd.m_verticalAttractionEfficiency); + XWfloat("VATIME", vd.m_verticalAttractionTimescale); + + XWQuat("REF_FRAME", vd.m_referenceFrame); + + writer.WriteEndElement(); + writer = null; + } + } +} \ No newline at end of file diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs index 1cce4c0..3427199 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs @@ -115,90 +115,6 @@ namespace OpenSim.Region.Framework.Scenes private bool m_suspendUpdates; private List m_linkedAvatars = new List(); - private SOGVehicle m_vehicle = null; - - public int VehicleType - { - get - { - if (m_vehicle == null) - return (int)Vehicle.TYPE_NONE; - else - return (int)m_vehicle.Type; - } - set - { - m_vehicle = null; - if (value == (int)Vehicle.TYPE_NONE) - { - if (RootPart.PhysActor != null) - RootPart.PhysActor.VehicleType = (int)Vehicle.TYPE_NONE; - return; - } - m_vehicle = new SOGVehicle(); - m_vehicle.ProcessTypeChange((Vehicle)value); - { - if (RootPart.PhysActor != null) - RootPart.PhysActor.VehicleType = value; - return; - } - - } - } - - public void SetVehicleFlags(int param, bool remove) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessVehicleFlags(param, remove); - - if (RootPart.PhysActor != null) - { - RootPart.PhysActor.VehicleFlags(param, remove); - } - } - - public void SetVehicleFloatParam(int param, float value) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); - - if (RootPart.PhysActor != null) - { - RootPart.PhysActor.VehicleFloatParam(param, value); - } - } - - public void SetVehicleVectorParam(int param, Vector3 value) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); - - if (RootPart.PhysActor != null) - { - RootPart.PhysActor.VehicleVectorParam(param, value); - } - } - - public void SetVehicleRotationParam(int param, Quaternion rotation) - { - if (m_vehicle == null) - return; - - m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); - - if (RootPart.PhysActor != null) - { - RootPart.PhysActor.VehicleRotationParam(param, rotation); - } - } - - public bool areUpdatesSuspended { get @@ -1782,8 +1698,6 @@ namespace OpenSim.Region.Framework.Scenes // ResetChildPrimPhysicsPositions(); if (m_rootPart.PhysActor != null) { - if (m_vehicle != null) - m_vehicle.SetVehicle(m_rootPart.PhysActor); m_rootPart.PhysActor.Building = false; } } @@ -1791,10 +1705,6 @@ namespace OpenSim.Region.Framework.Scenes { // Apply physics to the root prim m_rootPart.ApplyPhysics(m_rootPart.GetEffectiveObjectFlags(), m_rootPart.VolumeDetectActive, false); - if (m_rootPart.PhysActor != null && m_vehicle != null) - { - m_vehicle.SetVehicle(m_rootPart.PhysActor); - } } } diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs index 6438694..eb59ffd 100644 --- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs +++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs @@ -310,6 +310,9 @@ namespace OpenSim.Region.Framework.Scenes private UUID m_collisionSound; private float m_collisionSoundVolume; + + private SOPVehicle m_vehicle = null; + #endregion Fields // ~SceneObjectPart() @@ -1556,8 +1559,14 @@ namespace OpenSim.Region.Framework.Scenes { PhysActor.SOPName = this.Name; // save object into the PhysActor so ODE internals know the joint/body info PhysActor.SetMaterial(Material); + + // if root part apply vehicle + if (m_vehicle != null && LocalId == ParentGroup.RootPart.LocalId) + m_vehicle.SetVehicle(PhysActor); + DoPhysicsPropertyUpdate(RigidBody, true); PhysActor.SetVolumeDetect(VolumeDetectActive ? 1 : 0); + if (!building) PhysActor.Building = false; } @@ -3168,39 +3177,90 @@ namespace OpenSim.Region.Framework.Scenes } } + + public int VehicleType + { + get + { + if (m_vehicle == null) + return (int)Vehicle.TYPE_NONE; + else + return (int)m_vehicle.Type; + } + set + { + SetVehicleType(value); + } + } + public void SetVehicleType(int type) { - if (ParentGroup.IsDeleted) - return; - ParentGroup.VehicleType = type; + m_vehicle = null; + if (type == (int)Vehicle.TYPE_NONE) + { + if (_parentID ==0 && PhysActor != null) + PhysActor.VehicleType = (int)Vehicle.TYPE_NONE; + return; + } + m_vehicle = new SOPVehicle(); + m_vehicle.ProcessTypeChange((Vehicle)type); + { + if (_parentID ==0 && PhysActor != null) + PhysActor.VehicleType = type; + return; + } } public void SetVehicleFlags(int param, bool remove) { - if (ParentGroup.IsDeleted) + if (m_vehicle == null) return; - ParentGroup.SetVehicleFlags(param, remove); + + m_vehicle.ProcessVehicleFlags(param, remove); + + if (_parentID ==0 && PhysActor != null) + { + PhysActor.VehicleFlags(param, remove); + } } public void SetVehicleFloatParam(int param, float value) { - if (ParentGroup.IsDeleted) + if (m_vehicle == null) return; - ParentGroup.SetVehicleFloatParam(param, value); + + m_vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + + if (_parentID == 0 && PhysActor != null) + { + PhysActor.VehicleFloatParam(param, value); + } } public void SetVehicleVectorParam(int param, Vector3 value) { - if (ParentGroup.IsDeleted) + if (m_vehicle == null) return; - ParentGroup.SetVehicleVectorParam(param, value); + + m_vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + + if (_parentID == 0 && PhysActor != null) + { + PhysActor.VehicleVectorParam(param, value); + } } public void SetVehicleRotationParam(int param, Quaternion rotation) { - if (ParentGroup.IsDeleted) + if (m_vehicle == null) return; - ParentGroup.SetVehicleRotationParam(param, rotation); + + m_vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + + if (_parentID == 0 && PhysActor != null) + { + PhysActor.VehicleRotationParam(param, rotation); + } } /// @@ -4380,6 +4440,11 @@ namespace OpenSim.Region.Framework.Scenes m_localId); PhysActor.SetMaterial(Material); + + // if root part apply vehicle + if (m_vehicle != null && LocalId == ParentGroup.RootPart.LocalId) + m_vehicle.SetVehicle(PhysActor); + DoPhysicsPropertyUpdate(UsePhysics, true); if (!ParentGroup.IsDeleted) -- cgit v1.1