From e592d038430ab994ed65df4f66e0cf5c32af476c Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Sun, 5 Apr 2009 08:35:38 +0000 Subject: * Committing what I have on the BulletDotNETPlugin that I have so far. * It's not ready to try. It doesn't do what you expect in many ways. * It throws errors and prints jibberish on the console * Test it out only if you're brave and you've backed up first. * The opensim.ini line is physics = BulletDotNETPlugin --- .../Physics/BulletDotNETPlugin/AssemblyInfo.cs | 58 + .../BulletDotNETPlugin/BulletDotNETCharacter.cs | 1104 ++++++++++ .../BulletDotNETPlugin/BulletDotNETPlugin.cs | 65 + .../BulletDotNETPluginStructs.cs | 33 + .../Physics/BulletDotNETPlugin/BulletDotNETPrim.cs | 2131 ++++++++++++++++++++ .../BulletDotNETPlugin/BulletDotNETScene.cs | 642 ++++++ 6 files changed, 4033 insertions(+) create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/AssemblyInfo.cs create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPlugin.cs create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPluginStructs.cs create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs create mode 100644 OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs (limited to 'OpenSim/Region/Physics') diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/AssemblyInfo.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/AssemblyInfo.cs new file mode 100644 index 0000000..d10c8ad --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/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 OpenSim 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("BulletDotNETPlugin")] +[assembly : AssemblyDescription("")] +[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.3.*")] diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs new file mode 100644 index 0000000..6791671 --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs @@ -0,0 +1,1104 @@ +using System; +using System.Reflection; +using BulletDotNET; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.BulletDotNETPlugin +{ + public class BulletDotNETCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + public btRigidBody Body; + public btCollisionShape Shell; + public btVector3 tempVector1; + public btVector3 tempVector2; + public btVector3 tempVector3; + public btVector3 tempVector4; + + public btVector3 tempVector5RayCast; + public btVector3 tempVector6RayCast; + public btVector3 tempVector7RayCast; + + public btQuaternion tempQuat1; + public btTransform tempTrans1; + + public ClosestNotMeRayResultCallback ClosestCastResult; + private btTransform m_bodyTransform; + private btVector3 m_bodyPosition; + private btVector3 m_CapsuleOrientationAxis; + private btQuaternion m_bodyOrientation; + private btDefaultMotionState m_bodyMotionState; + private btGeneric6DofConstraint m_aMotor; + private PhysicsVector m_movementComparision; + private PhysicsVector m_position; + private PhysicsVector m_zeroPosition; + private bool m_zeroFlag = false; + private bool m_lastUpdateSent = false; + private PhysicsVector m_velocity; + private PhysicsVector m_target_velocity; + private PhysicsVector m_acceleration; + private PhysicsVector m_rotationalVelocity; + private bool m_pidControllerActive = true; + public float PID_D = 80.0f; + public float PID_P = 90.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float heightFudgeFactor = 0.52f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_wascolliding = false; + private bool m_wascollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private bool m_hackSentFall = false; + private bool m_hackSentFly = 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) + private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. + private bool m_taintRemove = false; + private bool m_taintedPosition = false; + private PhysicsVector m_taintedPosition_value; + private PhysicsVector m_taintedForce; + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + + private bool[] m_colliderarr = new bool[11]; + private bool[] m_colliderGroundarr = new bool[11]; + + + + private BulletDotNETScene m_parent_scene; + + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) + { + m_taintedForce = new PhysicsVector(); + m_velocity = new PhysicsVector(); + m_target_velocity = new PhysicsVector(); + m_position = pos; + m_zeroPosition = new PhysicsVector(pos.X, pos.Y, pos.Z); // this is a class, not a struct. Must make new, or m_zeroPosition will == position regardless + m_acceleration = new PhysicsVector(); + m_parent_scene = parent_scene; + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_density = density; + heightFudgeFactor = height_fudge_factor; + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + for (int i = 0; i < 11; i++) + { + m_colliderarr[i] = false; + } + for (int i = 0; i < 11; i++) + { + m_colliderGroundarr[i] = false; + } + CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + 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_parent_scene.AddPhysicsActorTaint(this); + + m_name = avName; + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + + + + } + + /// + /// 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) + { + + 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 = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH); + + if (m_bodyPosition == null) + m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ); + + m_bodyPosition.setValue(npositionX, npositionY, npositionZ); + + if (m_bodyOrientation == null) + m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90)); + + if (m_bodyTransform == null) + m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); + else + { + m_bodyTransform.Dispose(); + m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); + } + + if (m_bodyMotionState == null) + m_bodyMotionState = new btDefaultMotionState(m_bodyTransform); + else + m_bodyMotionState.setWorldTransform(m_bodyTransform); + + m_mass = Mass; + + Body = new btRigidBody(m_mass, m_bodyMotionState, Shell); + Body.setUserPointer(new IntPtr((int)Body.Handle)); + + if (ClosestCastResult != null) + ClosestCastResult.Dispose(); + ClosestCastResult = new ClosestNotMeRayResultCallback(Body); + + m_parent_scene.AddRigidBody(Body); + Body.setActivationState(4); + if (m_aMotor != null) + { + if (m_aMotor.Handle != IntPtr.Zero) + { + m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); + m_aMotor.Dispose(); + } + m_aMotor = null; + } + + m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody, + m_parent_scene.TransZero, + m_parent_scene.TransZero, false); + m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero); + m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero); + + + } + public void Remove() + { + m_taintRemove = true; + } + public override bool Stopped + { + get { return m_zeroFlag; } + } + + public override PhysicsVector Size + { + get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } + set + { + m_pidControllerActive = true; + + PhysicsVector SetSize = value; + m_tainted_CAPSULE_LENGTH = (SetSize.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + + Velocity = new PhysicsVector(0f, 0f, 0f); + + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + /// + /// 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 PrimitiveBaseShape Shape + { + set { return; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + + public override void CrossingFailure() + { + + } + + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(PhysicsVector axis) + { + + } + + public override PhysicsVector Position + { + get { return m_position; } + set + { + m_taintedPosition_value = value; + m_position = value; + m_taintedPosition = true; + } + } + + public override float Mass + { + get + { + float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH); + return m_density * AVvolume; + } + } + + public override PhysicsVector Force + { + get { return new PhysicsVector(m_target_velocity.X, m_target_velocity.Y, m_target_velocity.Z); } + 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, PhysicsVector value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override PhysicsVector GeometricCenter + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector CenterOfMass + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector Velocity + { + get + { + // There's a problem with PhysicsVector.Zero! Don't Use it Here! + if (m_zeroFlag) + return new PhysicsVector(0f, 0f, 0f); + m_lastUpdateSent = false; + return m_velocity; + } + set + { + m_pidControllerActive = true; + m_target_velocity = value; + } + } + + public override PhysicsVector Torque + { + get { return PhysicsVector.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override PhysicsVector Acceleration + { + get { return m_acceleration; } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set + { + + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return m_flying; } + set { m_flying = value; } + } + + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderarr[i] = m_colliderarr[i + 1]; + } + } + m_colliderarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + m_log.DebugFormat("[PHYSICS]: TrueCount:{0}, FalseCount:{1}",truecount,falsecount); + if (falsecount > 1.2 * truecount) + { + m_iscolliding = false; + } + else + { + m_iscolliding = true; + } + if (m_wascolliding != m_iscolliding) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascolliding = m_iscolliding; + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + // Collisions against the ground are not really reliable + // So, to get a consistant value we have to average the current result over time + // Currently we use 1 second = 10 calls to this. + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderGroundarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; + } + } + m_colliderGroundarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderGroundarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2 * truecount) + { + m_iscollidingGround = false; + } + else + { + m_iscollidingGround = true; + } + if (m_wascollidingGround != m_iscollidingGround) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascollidingGround = m_iscollidingGround; + } + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + m_iscollidingObj = value; + if (value) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + } + } + + + public override bool FloatOnWater + { + set { return; } + } + + public override PhysicsVector RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override bool PIDHoverActive + { + set { return; } + } + + public override float PIDHoverHeight + { + set { return; } + } + + public override PIDHoverType PIDHoverType + { + set { return; } + } + + public override float PIDHoverTau + { + set { return; } + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + /// Is this a push by a script? + public override void AddForce(PhysicsVector force, bool pushforce) + { + if (pushforce) + { + m_pidControllerActive = false; + force *= 100f; + doForce(force, false); + //System.Console.WriteLine("Push!"); + //_target_velocity.X += force.X; + // _target_velocity.Y += force.Y; + //_target_velocity.Z += force.Z; + } + else + { + m_pidControllerActive = true; + m_target_velocity.X += force.X; + m_target_velocity.Y += force.Y; + m_target_velocity.Z += force.Z; + } + //m_lastUpdateSent = false; + } + + public void doForce(PhysicsVector force, bool now) + { + + tempVector3.setValue(force.X, force.Y, force.Z); + if (now) + { + Body.applyCentralForce(tempVector3); + } + else + { + m_taintedForce += force; + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + public void doImpulse(PhysicsVector force, bool now) + { + + tempVector3.setValue(force.X, force.Y, force.Z); + if (now) + { + Body.applyCentralImpulse(tempVector3); + } + else + { + m_taintedForce += force; + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + public override void AddAngularForce(PhysicsVector force, bool pushforce) + { + + } + + public override void SetMomentum(PhysicsVector momentum) + { + + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_parent_scene.addCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + m_parent_scene.remCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + internal void Dispose() + { + if (Body.isInWorld()) + m_parent_scene.removeFromWorld(Body); + + if (m_aMotor.Handle != IntPtr.Zero) + m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); + + m_aMotor.Dispose(); m_aMotor = null; + ClosestCastResult.Dispose(); ClosestCastResult = null; + Body.Dispose(); Body = null; + Shell.Dispose(); Shell = null; + tempQuat1.Dispose(); + tempTrans1.Dispose(); + tempVector1.Dispose(); + tempVector2.Dispose(); + tempVector3.Dispose(); + tempVector4.Dispose(); + tempVector5RayCast.Dispose(); + tempVector6RayCast.Dispose(); + + } + + public void ProcessTaints(float timestep) + { + + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) + { + // Create avatar capsule and related ODE data + if (!(Shell == null && Body == null)) + { + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell != null ? "Shell " : "") + + (Body != null ? "Body " : "")); + } + AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z); + + + } + else + { + // destroy avatar capsule and related ODE data + + Dispose(); + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + } + + m_isPhysical = m_tainted_isPhysical; + } + + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + { + if (Body != null) + { + + m_pidControllerActive = true; + // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() + //d.JointDestroy(Amotor); + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + Dispose(); + + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + + AvatarGeomAndBodyCreation(m_position.X, m_position.Y, + m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + Velocity = new PhysicsVector(0f, 0f, 0f); + + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == null ? "Shell " : "") + + (Body == null ? "Body " : "")); + } + } + if (m_taintRemove) + { + Dispose(); + } + } + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep) + { + // 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 == null) + return; + tempTrans1.Dispose(); + tempTrans1 = Body.getInterpolationWorldTransform(); + tempVector1.Dispose(); + tempVector1 = tempTrans1.getOrigin(); + tempVector2.Dispose(); + tempVector2 = Body.getInterpolationLinearVelocity(); + + if (m_pidControllerActive == false) + { + m_zeroPosition.X = tempVector1.getX(); + m_zeroPosition.Y = tempVector1.getY(); + m_zeroPosition.Z = tempVector1.getZ(); + } + //PidStatus = true; + + PhysicsVector vec = new PhysicsVector(); + + PhysicsVector vel = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); + + float movementdivisor = 1f; + + if (!m_alwaysRun) + { + movementdivisor = walkDivisor; + } + else + { + movementdivisor = runDivisor; + } + + // if velocity is zero, use position control; otherwise, velocity control + if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!m_zeroFlag) + { + m_zeroFlag = true; + m_zeroPosition.X = tempVector1.getX(); + m_zeroPosition.Y = tempVector1.getY(); + m_zeroPosition.Z = tempVector1.getZ(); + } + 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 + + PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); + vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2); + vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2); + if (m_flying) + { + vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + m_zeroFlag = false; + if (m_iscolliding && !m_flying) + { + // We're standing on something + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); + } + else if (m_iscolliding && m_flying) + { + // We're flying and colliding with something + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16); + } + else if (!m_iscolliding && m_flying) + { + // we're in mid air suspended + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6); + } + + if (m_iscolliding && !m_flying && m_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. + PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); + vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P; + if (m_target_velocity.X > 0) + { + vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (m_target_velocity.Y > 0) + { + vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + else if (!m_iscolliding && !m_flying) + { + // 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); + if (m_target_velocity.X > 0) + { + vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (m_target_velocity.Y > 0) + { + vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + + + if (m_flying) + { + vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D); + } + } + if (m_flying) + { + //vec.Z += ((-1 * m_parent_scene.gravityz) * m_mass); + } + if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f)))) + { + Body.setFriction(0.001f); + //m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString()); + } + + if (Body != null) + { + int activationstate = Body.getActivationState(); + if (activationstate == 0) + { + Body.forceActivationState(1); + } + + + } + doImpulse(vec, true); + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + if (Body == null) + return; + //int val = Environment.TickCount; + CheckIfStandingOnObject(); + //m_log.DebugFormat("time:{0}", Environment.TickCount - val); + + //IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody); + + tempTrans1.Dispose(); + tempTrans1 = Body.getInterpolationWorldTransform(); + tempVector1.Dispose(); + tempVector1 = tempTrans1.getOrigin(); + tempVector2.Dispose(); + tempVector2 = Body.getInterpolationLinearVelocity(); + + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + PhysicsVector vec = new PhysicsVector(tempVector1.getX(),tempVector1.getY(),tempVector1.getZ()); + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + if (vec.X < 0.0f) vec.X = 0.0f; + if (vec.Y < 0.0f) vec.Y = 0.0f; + if (vec.X > (int)Constants.RegionSize - 0.2f) vec.X = (int)Constants.RegionSize - 0.2f; + if (vec.Y > (int)Constants.RegionSize - 0.2f) vec.Y = (int)Constants.RegionSize - 0.2f; + + m_position.X = vec.X; + m_position.Y = vec.Y; + m_position.Z = vec.Z; + + // Did we move last? = zeroflag + // This helps keep us from sliding all over + + if (m_zeroFlag) + { + m_velocity.X = 0.0f; + m_velocity.Y = 0.0f; + m_velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + //base.RequestPhysicsterseUpdate(); + + } + } + else + { + m_lastUpdateSent = false; + vec = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); + m_velocity.X = (vec.X); + m_velocity.Y = (vec.Y); + + m_velocity.Z = (vec.Z); + + if (m_velocity.Z < -6 && !m_hackSentFall) + { + m_hackSentFall = true; + m_pidControllerActive = false; + } + else if (m_flying && !m_hackSentFly) + { + //m_hackSentFly = true; + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else + { + m_hackSentFly = false; + m_hackSentFall = false; + } + } + if (Body != null) + { + if (Body.getFriction() < 0.9f) + Body.setFriction(0.9f); + } + //if (Body != null) + // Body.clearForces(); + } + + public void CheckIfStandingOnObject() + { + + float capsuleHalfHeight = ((CAPSULE_LENGTH + 2*CAPSULE_RADIUS)*0.5f); + + tempVector5RayCast.setValue(m_position.X, m_position.Y, m_position.Z); + tempVector6RayCast.setValue(m_position.X, m_position.Y, m_position.Z - 1 * capsuleHalfHeight * 1.1f); + + + ClosestCastResult.Dispose(); + ClosestCastResult = new ClosestNotMeRayResultCallback(Body); + + try + { + m_parent_scene.getBulletWorld().rayTest(tempVector5RayCast, tempVector6RayCast, ClosestCastResult); + } + catch (AccessViolationException) + { + m_log.Debug("BAD!"); + } + if (ClosestCastResult.hasHit()) + { + + if (tempVector7RayCast != null) + tempVector7RayCast.Dispose(); + + //tempVector7RayCast = ClosestCastResult.getHitPointWorld(); + + /*if (tempVector7RayCast == null) // null == no result also + { + CollidingObj = false; + IsColliding = false; + CollidingGround = false; + + return; + } + float zVal = tempVector7RayCast.getZ(); + if (zVal != 0) + m_log.Debug("[PHYSICS]: HAAAA"); + if (zVal < m_position.Z && zVal > ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) *0.5f)) + { + CollidingObj = true; + IsColliding = true; + } + else + { + CollidingObj = false; + IsColliding = false; + CollidingGround = false; + }*/ + + //height+2*radius = capsule full length + //CollidingObj = true; + //IsColliding = true; + m_iscolliding = true; + } + else + { + //CollidingObj = false; + //IsColliding = false; + //CollidingGround = false; + m_iscolliding = false; + } + } + } + +} diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPlugin.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPlugin.cs new file mode 100644 index 0000000..262e5c3 --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPlugin.cs @@ -0,0 +1,65 @@ +/* + * 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 OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.BulletDotNETPlugin +{ + public class BulletDotNetPlugin : IPhysicsPlugin + { + private BulletDotNETScene m_scene; + private const string m_pluginName = "BulletDotNETPlugin"; + + #region IPhysicsPlugin Members + + public bool Init() + { + return true; + } + + public PhysicsScene GetScene(string sceneIdentifier) + { + if (m_scene == null) + { + m_scene = new BulletDotNETScene(sceneIdentifier); + } + return m_scene; + } + + public string GetName() + { + return m_pluginName; + } + + public void Dispose() + { + + } + + #endregion + } +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPluginStructs.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPluginStructs.cs new file mode 100644 index 0000000..382f445 --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPluginStructs.cs @@ -0,0 +1,33 @@ +using System; + +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 +} \ No newline at end of file diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs new file mode 100644 index 0000000..3e26456 --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs @@ -0,0 +1,2131 @@ +/* + * 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 OpenSim 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 log4net; +using OpenMetaverse; +using BulletDotNET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.BulletDotNETPlugin +{ + public class BulletDotNETPrim : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private PhysicsVector _position; + private PhysicsVector _velocity; + private PhysicsVector _torque = new PhysicsVector(0, 0, 0); + private PhysicsVector m_lastVelocity = new PhysicsVector(0.0f, 0.0f, 0.0f); + private PhysicsVector m_lastposition = new PhysicsVector(0.0f, 0.0f, 0.0f); + private Quaternion m_lastorientation = new Quaternion(); + private PhysicsVector m_rotationalVelocity; + private PhysicsVector _size; + private PhysicsVector _acceleration; + // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); + private Quaternion _orientation; + private PhysicsVector m_taintposition; + private PhysicsVector m_taintsize; + private PhysicsVector m_taintVelocity = new PhysicsVector(0, 0, 0); + private PhysicsVector m_taintTorque = new PhysicsVector(0, 0, 0); + private Quaternion m_taintrot; + private PhysicsVector m_angularlock = new PhysicsVector(1f, 1f, 1f); + private PhysicsVector m_taintAngularLock = new PhysicsVector(1f, 1f, 1f); + private btGeneric6DofConstraint Amotor; + + private PhysicsVector m_PIDTarget = new PhysicsVector(0, 0, 0); + private float m_PIDTau = 0f; + private float m_PIDHoverHeight = 0f; + private float m_PIDHoverTau = 0f; + private bool m_useHoverPID = false; + private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; + private float m_targetHoverHeight = 0f; + private float m_groundHeight = 0f; + private float m_waterHeight = 0f; + private float PID_D = 35f; + private float PID_G = 25f; + private float m_tensor = 5f; + private int body_autodisable_frames = 20; + private IMesh primMesh = null; + + private bool m_usePID = false; + + + private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom + | CollisionCategories.Space + | CollisionCategories.Body + | CollisionCategories.Character + ); + + private bool m_taintshape = false; + private bool m_taintPhysics = false; + private bool m_collidesLand = true; + private bool m_collidesWater = false; + public bool m_returnCollisions = false; + + // 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_taintremove = false; + public bool m_taintdisable = false; + public bool m_disabled = false; + public bool m_taintadd = false; + public bool m_taintselected = false; + public bool m_taintCollidesWater = false; + + public uint m_localID = 0; + + //public GCHandle gc; + private CollisionLocker ode; + + private bool m_taintforce = false; + private bool m_taintaddangularforce = false; + private PhysicsVector m_force = new PhysicsVector(0.0f, 0.0f, 0.0f); + private List m_forcelist = new List(); + private List m_angularforcelist = new List(); + + private IMesh _mesh; + private PrimitiveBaseShape _pbs; + private BulletDotNETScene _parent_scene; + public btCollisionShape prim_geom; + public IntPtr _triMeshData; + + private PhysicsActor _parent = null; + private PhysicsActor m_taintparent = null; + + private List childrenPrim = new List(); + + private bool iscolliding = false; + private bool m_isphysical = false; + private bool m_isSelected = false; + + internal bool m_isVolumeDetect = false; // If true, this prim only detects collisions but doesn't collide actively + + private bool m_throttleUpdates = false; + private int throttleCounter = 0; + public int m_interpenetrationcount = 0; + public float m_collisionscore = 0; + public int m_roundsUnderMotionThreshold = 0; + private int m_crossingfailures = 0; + + public float m_buoyancy = 0f; + + public bool outofBounds = false; + private float m_density = 10.000006836f; // Aluminum g/cm3; + + public bool _zeroFlag = false; + private bool m_lastUpdateSent = false; + + + private String m_primName; + private PhysicsVector _target_velocity; + + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = null; + + public volatile bool childPrim = false; + + private btVector3 tempPosition1; + private btVector3 tempPosition2; + private btVector3 tempPosition3; + private btVector3 tempSize1; + private btVector3 tempSize2; + private btVector3 tempLinearVelocity1; + private btVector3 tempLinearVelocity2; + private btVector3 tempAngularVelocity1; + private btVector3 tempAngularVelocity2; + private btVector3 tempInertia1; + private btVector3 tempInertia2; + private btQuaternion tempOrientation1; + private btQuaternion tempOrientation2; + private btMotionState tempMotionState1; + private btMotionState tempMotionState2; + private btMotionState tempMotionState3; + private btTransform tempTransform1; + private btTransform tempTransform2; + private btTransform tempTransform3; + private btTransform tempTransform4; + private btTriangleIndexVertexArray btshapeArray; + + public btRigidBody Body; + + public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size, + Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical) + { + tempPosition1 = new btVector3(0, 0, 0); + tempPosition2 = new btVector3(0, 0, 0); + tempPosition3 = new btVector3(0, 0, 0); + tempSize1 = new btVector3(0, 0, 0); + tempSize2 = new btVector3(0, 0, 0); + tempLinearVelocity1 = new btVector3(0, 0, 0); + tempLinearVelocity2 = new btVector3(0, 0, 0); + tempAngularVelocity1 = new btVector3(0, 0, 0); + tempAngularVelocity2 = new btVector3(0, 0, 0); + tempInertia1 = new btVector3(0, 0, 0); + tempInertia2 = new btVector3(0, 0, 0); + tempOrientation1 = new btQuaternion(0,0,0,1); + tempOrientation2 = new btQuaternion(0, 0, 0, 1); + _parent_scene = parent_scene; + tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); + tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ; + tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ; + tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ; + + tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero); + tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero); + tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero); + + _target_velocity = new PhysicsVector(0, 0, 0); + _velocity = new PhysicsVector(); + _position = pos; + m_taintposition = pos; + 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 = null; + Body = null; + + 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; + m_taintsize = _size; + _acceleration = new PhysicsVector(); + m_rotationalVelocity = PhysicsVector.Zero; + _orientation = rotation; + m_taintrot = _orientation; + _mesh = mesh; + _pbs = pbs; + + _parent_scene = parent_scene; + + if (pos.Z < 0) + m_isphysical = false; + else + { + m_isphysical = pisPhysical; + // If we're physical, we need to be in the master space for now. + // linksets *should* be in a space together.. but are not currently + } + m_primName = primName; + m_taintadd = true; + _parent_scene.AddPhysicsActorTaint(this); + + } + + #region PhysicsActor overrides + + public override bool Stopped + { + get { return _zeroFlag; } + } + + public override PhysicsVector Size + { + get { return _size; } + set { _size = value; } + } + + public override PrimitiveBaseShape Shape + { + set + { + _pbs = value; + m_taintshape = true; + } + } + + public override uint LocalID + { + set + { + //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); + m_localID = value; + } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set + { + // This only makes the object not collidable if the object + // is physical or the object is modified somehow *IN THE FUTURE* + // without this, if an avatar selects prim, they can walk right + // through it while it's selected + m_collisionscore = 0; + if ((m_isphysical && !_zeroFlag) || !value) + { + m_taintselected = value; + _parent_scene.AddPhysicsActorTaint(this); + } + else + { + m_taintselected = value; + m_isSelected = value; + } + } + } + + public override void CrossingFailure() + { + m_crossingfailures++; + if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + base.RaiseOutOfBounds(_position); + return; + } + else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); + } + } + public override void link(PhysicsActor obj) + { + //TODO: + } + + public override void delink() + { + //TODO: + } + + public override void LockAngularMotion(PhysicsVector axis) + { + m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); + m_taintAngularLock = new PhysicsVector(axis.X, axis.Y, axis.Z); + } + + public override PhysicsVector Position + { + get { return _position; } + + set + { + _position = value; + //m_log.Info("[PHYSICS]: " + _position.ToString()); + } + } + + public override float Mass + { + get { return CalculateMass(); } + } + + public override PhysicsVector Force + { + //get { return PhysicsVector.Zero; } + get { return m_force; } + set { m_force = value; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + //TODO: + } + + public override void VehicleVectorParam(int param, PhysicsVector value) + { + //TODO: + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + //TODO: + } + + public override void SetVolumeDetect(int param) + { + //TODO: GhostObject + m_isVolumeDetect = (param != 0); + + } + + public override PhysicsVector GeometricCenter + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector CenterOfMass + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector Velocity + { + get + { + // Averate previous velocity with the new one so + // client object interpolation works a 'little' better + PhysicsVector returnVelocity = new PhysicsVector(); + 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 + { + _velocity = value; + + m_taintVelocity = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } + + public override PhysicsVector Torque + { + get + { + if (!m_isphysical || Body.Handle == IntPtr.Zero) + return new PhysicsVector(0, 0, 0); + + return _torque; + } + + set + { + m_taintTorque = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } + + public override float CollisionScore + { + get { return m_collisionscore; } + set { m_collisionscore = value; } + } + + public override PhysicsVector Acceleration + { + get { return _acceleration; } + } + + public override Quaternion Orientation + { + get { return _orientation; } + set { _orientation = value; } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Prim; } + set { return; } + } + + public override bool IsPhysical + { + get { return m_isphysical; } + set { m_isphysical = value; } + } + + public override bool Flying + { + // no flying prims for you + get { return false; } + set { } + } + + public override bool SetAlwaysRun + { + get { return false; } + set { return; } + } + + public override bool ThrottleUpdates + { + get { return m_throttleUpdates; } + set { m_throttleUpdates = value; } + } + + public override bool IsColliding + { + get { return iscolliding; } + set { iscolliding = value; } + } + + public override bool CollidingGround + { + get { return false; } + set { return; } + } + + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override bool FloatOnWater + { + set + { + m_taintCollidesWater = value; + _parent_scene.AddPhysicsActorTaint(this); + } + } + + public override PhysicsVector RotationalVelocity + { + get + { + PhysicsVector pv = new PhysicsVector(0, 0, 0); + if (_zeroFlag) + return pv; + m_lastUpdateSent = false; + + if (m_rotationalVelocity.IsIdentical(pv, 0.2f)) + return pv; + + return m_rotationalVelocity; + } + set { m_rotationalVelocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override PhysicsVector PIDTarget { set { m_PIDTarget = value; ; } } + 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 void AddForce(PhysicsVector force, bool pushforce) + { + m_forcelist.Add(force); + m_taintforce = true; + //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); + } + + public override void AddAngularForce(PhysicsVector force, bool pushforce) + { + m_angularforcelist.Add(force); + m_taintaddangularforce = true; + } + + public override void SetMomentum(PhysicsVector momentum) + { + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + _parent_scene.addCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + _parent_scene.remCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public override bool SubscribedEvents() + { + return (m_eventsubscription > 0); + } + + #endregion + + + + internal void Dispose() + { + //TODO: + DisposeOfBody(); + SetCollisionShape(null); + + if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero) + { + tempMotionState3.Dispose(); + tempMotionState3 = null; + } + + if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero) + { + tempMotionState2.Dispose(); + tempMotionState2 = null; + } + + if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero) + { + tempMotionState1.Dispose(); + tempMotionState1 = null; + } + + if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero) + { + tempTransform4.Dispose(); + tempTransform4 = null; + } + + if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero) + { + tempTransform3.Dispose(); + tempTransform3 = null; + } + + if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero) + { + tempTransform2.Dispose(); + tempTransform2 = null; + } + + if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero) + { + tempTransform1.Dispose(); + tempTransform1 = null; + } + + if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero) + { + tempOrientation2.Dispose(); + tempOrientation2 = null; + } + + if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero) + { + tempOrientation1.Dispose(); + tempOrientation1 = null; + } + + if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero) + { + tempInertia1.Dispose(); + tempInertia1 = null; + } + + if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero) + { + tempInertia2.Dispose(); + tempInertia1 = null; + } + + + if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero) + { + tempAngularVelocity2.Dispose(); + tempAngularVelocity2 = null; + } + + if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero) + { + tempAngularVelocity1.Dispose(); + tempAngularVelocity1 = null; + } + + if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero) + { + tempLinearVelocity2.Dispose(); + tempLinearVelocity2 = null; + } + + if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero) + { + tempLinearVelocity1.Dispose(); + tempLinearVelocity1 = null; + } + + if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero) + { + tempSize2.Dispose(); + tempSize2 = null; + } + + if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero) + { + tempSize1.Dispose(); + tempSize1 = null; + } + + if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero) + { + tempPosition3.Dispose(); + tempPosition3 = null; + } + + if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero) + { + tempPosition2.Dispose(); + tempPosition2 = null; + } + + if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero) + { + tempPosition1.Dispose(); + tempPosition1 = null; + } + + } + + + + public void ProcessTaints(float timestep) + { + if (m_taintadd) + { + changeadd(timestep); + } + + if (prim_geom.Handle == IntPtr.Zero) + { + CreateGeom(IntPtr.Zero, primMesh); + + if (IsPhysical) + SetBody(Mass); + else + SetBody(0); + + } + + if (!_position.IsIdentical(m_taintposition, 0f)) + changemove(timestep); + + if (m_taintrot != _orientation) + rotate(timestep); + // + + if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) + changePhysicsStatus(timestep); + // + + if (!_size.IsIdentical(m_taintsize, 0)) + changesize(timestep); + // + + if (m_taintshape) + changeshape(timestep); + // + + if (m_taintforce) + changeAddForce(timestep); + + if (m_taintaddangularforce) + changeAddAngularForce(timestep); + + if (!m_taintTorque.IsIdentical(PhysicsVector.Zero, 0.001f)) + changeSetTorque(timestep); + + if (m_taintdisable) + changedisable(timestep); + + if (m_taintselected != m_isSelected) + changeSelectedStatus(timestep); + + if (!m_taintVelocity.IsIdentical(PhysicsVector.Zero, 0.001f)) + changevelocity(timestep); + + if (m_taintparent != _parent) + changelink(timestep); + + if (m_taintCollidesWater != m_collidesWater) + changefloatonwater(timestep); + + if (!m_angularlock.IsIdentical(m_taintAngularLock, 0)) + changeAngularLock(timestep); + + + } + + #region Physics Scene Change Action routines + + private void changeadd(float timestep) + { + if (_mesh == null) + { + if (_parent_scene.needsMeshing(_pbs)) + { + // Don't need to re-enable body.. it's done in SetMesh + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + // createmesh returns null when it's a shape that isn't a cube. + } + } + CreateGeom(IntPtr.Zero, primMesh); + + enableBody(); + changeSelectedStatus(timestep); + m_taintadd = false; + + } + + private void changemove(float timestep) + { + + + tempTransform2 = Body.getWorldTransform(); + btQuaternion quat = tempTransform2.getRotation(); + tempPosition2.setValue(_position.X, _position.Y, _position.Z); + tempTransform2.Dispose(); + tempTransform2 = new btTransform(quat, tempPosition2); + Body.setWorldTransform(tempTransform2); + + changeSelectedStatus(timestep); + + resetCollisionAccounting(); + m_taintposition = _position; + } + + private void rotate(float timestep) + { + + tempTransform2 = Body.getWorldTransform(); + tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W); + tempTransform2.setRotation(tempOrientation2); + Body.setWorldTransform(tempTransform2); + + resetCollisionAccounting(); + m_taintrot = _orientation; + } + + private void changePhysicsStatus(float timestep) + { + SetCollisionShape(prim_geom); + SetBody(Mass); + changeSelectedStatus(timestep); + + resetCollisionAccounting(); + m_taintPhysics = m_isphysical; + } + + private void changesize(float timestep) + { + SetCollisionShape(null); + // Construction of new prim + if (_parent_scene.needsMeshing(_pbs)) + { + float meshlod = _parent_scene.meshSculptLOD; + + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + // Don't need to re-enable body.. it's done in SetMesh + + IMesh mesh = null; + + if (_parent_scene.needsMeshing(_pbs)) + mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical); + + //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); + + CreateGeom(IntPtr.Zero, mesh); + + + } + else + { + _mesh = null; + CreateGeom(IntPtr.Zero, _mesh); + } + + if (IsPhysical) + SetBody(Mass); + else + SetBody(0); + + m_taintsize = _size; + + } + + private void changeshape(float timestep) + { + // Cleanup of old prim geometry and Bodies + if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + BulletDotNETPrim parent = (BulletDotNETPrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + try + { + SetCollisionShape(null); + } + catch (System.AccessViolationException) + { + //prim_geom = IntPtr.Zero; + m_log.Error("[PHYSICS]: PrimGeom dead"); + } + + // 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 + + if (_parent_scene.needsMeshing(_pbs)) + { + // Don't need to re-enable body.. it's done in SetMesh + float meshlod = _parent_scene.meshSculptLOD; + + if (IsPhysical) + meshlod = _parent_scene.MeshSculptphysicalLOD; + + IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical); + // createmesh returns null when it doesn't mesh. + CreateGeom(IntPtr.Zero, mesh); + } + else + { + _mesh = null; + CreateGeom(IntPtr.Zero, null); + } + tempPosition1.setValue(_position.X, _position.Y, _position.Z); + if (tempOrientation1.Handle != IntPtr.Zero) + tempOrientation1.Dispose(); + tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W); + if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero) + tempTransform1.Dispose(); + tempTransform1 = new btTransform(tempOrientation1, tempPosition1); + + + + + //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); + if (IsPhysical) + { + SetBody(Mass); + // Re creates body on size. + // EnableBody also does setMass() + + } + else + { + SetBody(0); + } + + changeSelectedStatus(timestep); + if (childPrim) + { + if (_parent is BulletDotNETPrim) + { + BulletDotNETPrim parent = (BulletDotNETPrim)_parent; + parent.ChildSetGeom(this); + } + } + resetCollisionAccounting(); + m_taintshape = false; + } + + private void resetCollisionAccounting() + { + m_collisionscore = 0; + } + + private void ChildSetGeom(BulletDotNETPrim bulletDotNETPrim) + { + // TODO: throw new NotImplementedException(); + } + + private void changeAddForce(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changeAddAngularForce(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changeSetTorque(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changedisable(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changeSelectedStatus(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changevelocity(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changelink(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changefloatonwater(float timestep) + { + // TODO: throw new NotImplementedException(); + } + + private void changeAngularLock(float timestep) + { + // TODO: throw new NotImplementedException(); + } + #endregion + + + + + internal void Move(float timestep) + { + //TODO: + float fx = 0; + float fy = 0; + float fz = 0; + + if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected) + { + float m_mass = CalculateMass(); + + fz = 0f; + //m_log.Info(m_collisionFlags.ToString()); + + if (m_buoyancy != 0) + { + if (m_buoyancy > 0) + { + fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass); + + //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); + //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); + } + else + { + fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass)); + } + } + + if (m_usePID) + { + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + 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; + } + + // TODO: NEED btVector3 for Linear Velocity + // NEED btVector3 for Position + + PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet + PhysicsVector vel = new PhysicsVector(0, 0, 0); + + _target_velocity = + new PhysicsVector( + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) + { + + /* TODO: Do Bullet equiv + * + 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; + + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + + } + + } + + if (m_useHoverPID && !m_usePID) + { + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + PhysicsVector pos = new PhysicsVector(0, 0, 0); //TODO: Insert values gotten from bullet + PhysicsVector vel = new PhysicsVector(0, 0, 0); + + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Absolute: + m_targetHoverHeight = m_PIDHoverHeight; + break; + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + case PIDHoverType.Water: + m_waterHeight = _parent_scene.GetWaterLevel(); + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + break; + } + + + _target_velocity = + new PhysicsVector(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.IsIdentical(PhysicsVector.Zero, 0.1f)) + { + + /* TODO: Do Bullet Equiv + 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 = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } + + 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) + { + /* + * TODO: Do Bullet Equiv + if (!d.BodyIsEnabled(Body)) + { + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } + */ + // 35x10 = 350n times the mass per second applied maximum. + + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; + + + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + + // TODO: Do Bullet Equiv + // d.BodyAddForce(Body, fx, fy, fz); + } + } + else + { + // _zeroPosition = d.BodyGetPosition(Body); + return; + } + } + + + + + #region Mass Calculation + + private float CalculateMass() + { + float volume = 0; + + // No material is passed to the physics engines yet.. soo.. + // we're using the m_density constant in the class definition + + float returnMass = 0; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // Profile Volume + + volume = _size.X * _size.Y * _size.Z; + + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + + if (((float)_pbs.ProfileHollow / 50000f) > 0.0) + { + float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + + break; + case ProfileShape.Circle: + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + // Cylinder + float volume1 = (float)(Math.PI * Math.Pow(_size.X / 2, 2) * _size.Z); + float volume2 = (float)(Math.PI * Math.Pow(_size.Y / 2, 2) * _size.Z); + + // Approximating the cylinder's irregularity. + if (volume1 > volume2) + { + volume = (float)volume1 - (volume1 - volume2); + } + else if (volume2 > volume1) + { + volume = (float)volume2 - (volume2 - volume1); + } + else + { + // Regular cylinder + volume = volume1; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + + if (((float)_pbs.ProfileHollow / 50000f) > 0.0) + { + float hollowAmount = (float)_pbs.ProfileHollow / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Z == _size.X) + { + // regular sphere + // v = 4/3 * pi * r^3 + float sradius3 = (float)Math.Pow((_size.X / 2), 3); + volume = (float)((4 / 3f) * Math.PI * sradius3); + } + else + { + // we treat this as a box currently + volume = _size.X * _size.Y * _size.Z; + } + } + else + { + // We don't know what the shape is yet, so use default + volume = _size.X * _size.Y * _size.Z; + } + break; + + case ProfileShape.EquilateralTriangle: + /* + v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h + + // seed mesh + Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f); + Vertex PM = new Vertex(+0.5f, 0f, 0.0f); + Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f); + */ + float xA = -0.25f * _size.X; + float yA = -0.45f * _size.Y; + + float xB = 0.5f * _size.X; + float yB = 0; + + float xC = -0.25f * _size.X; + float yC = 0.45f * _size.Y; + + volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z); + + // If the user has 'hollowed out' + // ProfileHollow is one of those 0 to 50000 values :P + // we like percentages better.. so turning into a percentage + float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f); + if (((float)fhollowFactor / 50000f) > 0.0) + { + float hollowAmount = (float)fhollowFactor / 50000f; + + // calculate the hollow volume by it's shape compared to the prim shape + float hollowVolume = 0; + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + // Equilateral Triangular Prism volume hollow calculation + // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y + + float aLength = _size.Y; + // 1/2 abh + hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount); + break; + + case HollowShape.Square: + // Cube Hollow volume calculation + float hollowsizex = _size.X * hollowAmount; + float hollowsizey = _size.Y * hollowAmount; + float hollowsizez = _size.Z * hollowAmount; + hollowVolume = hollowsizex * hollowsizey * hollowsizez; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + float hRadius = _size.X / 2; + float hLength = _size.Z; + + // pi * r2 * h + hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength) / 2) * hollowAmount); + break; + + default: + hollowVolume = 0; + break; + } + volume = volume - hollowVolume; + } + break; + + default: + // we don't have all of the volume formulas yet so + // use the common volume formula for all + volume = _size.X * _size.Y * _size.Z; + break; + } + + // Calculate Path cut effect on volume + // Not exact, in the triangle hollow example + // They should never be zero or less then zero.. + // we'll ignore it if it's less then zero + + // ProfileEnd and ProfileBegin are values + // from 0 to 50000 + + // Turning them back into percentages so that I can cut that percentage off the volume + + float PathCutEndAmount = _pbs.ProfileEnd; + float PathCutStartAmount = _pbs.ProfileBegin; + if (((PathCutStartAmount + PathCutEndAmount) / 50000f) > 0.0f) + { + float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f); + + // Check the return amount for sanity + if (pathCutAmount >= 0.99f) + pathCutAmount = 0.99f; + + volume = volume - (volume * pathCutAmount); + } + UInt16 taperX = _pbs.PathScaleX; + UInt16 taperY = _pbs.PathScaleY; + float taperFactorX = 0; + float taperFactorY = 0; + + // Mass = density * volume + if (taperX != 100) + { + if (taperX > 100) + { + taperFactorX = 1.0f - ((float)taperX / 200); + //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString()); + } + else + { + taperFactorX = 1.0f - ((100 - (float)taperX) / 100); + //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString()); + } + volume = (float)volume * ((taperFactorX / 3f) + 0.001f); + } + + if (taperY != 100) + { + if (taperY > 100) + { + taperFactorY = 1.0f - ((float)taperY / 200); + //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString()); + } + else + { + taperFactorY = 1.0f - ((100 - (float)taperY) / 100); + //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString()); + } + volume = (float)volume * ((taperFactorY / 3f) + 0.001f); + } + returnMass = m_density * volume; + if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. + + + + // Recursively calculate mass + bool HasChildPrim = false; + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + HasChildPrim = true; + } + + } + if (HasChildPrim) + { + BulletDotNETPrim[] childPrimArr = new BulletDotNETPrim[0]; + + lock (childrenPrim) + childPrimArr = childrenPrim.ToArray(); + + for (int i = 0; i < childPrimArr.Length; i++) + { + if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) + returnMass += childPrimArr[i].CalculateMass(); + // failsafe, this shouldn't happen but with OpenSim, you never know :) + if (i > 256) + break; + } + } + + + + + + return returnMass; + } + + #endregion + + + public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh) + { + if (p_mesh != null) + { + _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); + setMesh(_parent_scene, _mesh); + + } + else + { + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) + { + if (((_size.X / 2f) > 0f)) + { + //SetGeom to a Regular Sphere + tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f); + SetCollisionShape(new btSphereShape(_size.X*0.5f)); + } + else + { + // uses halfextents + tempSize1.setValue(_size.X*0.5f, _size.Y*0.5f, _size.Z*0.5f); + SetCollisionShape(new btBoxShape(tempSize1)); + } + } + else + { + // uses halfextents + tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f); + SetCollisionShape(new btBoxShape(tempSize1)); + } + + } + else + { + // uses halfextents + tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f); + SetCollisionShape(new btBoxShape(tempSize1)); + } + } + } + + private void setMesh(BulletDotNETScene _parent_scene, IMesh mesh) + { + // TODO: Set Collision Body Mesh + // This sleeper is there to moderate how long it takes between + // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object + + Thread.Sleep(10); + + //Kill Body so that mesh can re-make the geom + if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero) + { + if (childPrim) + { + if (_parent != null) + { + BulletDotNETPrim parent = (BulletDotNETPrim)_parent; + parent.ChildDelink(this); + } + } + else + { + disableBody(); + } + } + + IMesh oldMesh = primMesh; + + primMesh = mesh; + + float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory + int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage + //Array.Reverse(indexList); + primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory + + int VertexCount = vertexList.GetLength(0) / 3; + int IndexCount = indexList.GetLength(0); + + if (btshapeArray != null && btshapeArray.Handle != IntPtr.Zero) + btshapeArray.Dispose(); + //Array.Reverse(indexList); + btshapeArray = new btTriangleIndexVertexArray(IndexCount / 3, indexList, (3 * sizeof(int)), + VertexCount, vertexList, 3*sizeof (float)); + SetCollisionShape(new btGImpactMeshShape(btshapeArray)); + //((btGImpactMeshShape) prim_geom).updateBound(); + ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1,1, 1)); + ((btGImpactMeshShape)prim_geom).updateBound(); + _parent_scene.SetUsingGImpact(); + if (oldMesh != null) + { + oldMesh.releasePinned(); + oldMesh = null; + } + + } + + private void SetCollisionShape(btCollisionShape shape) + { + if (shape == null) + m_log.Debug("[PHYSICS]:SetShape!Null"); + else + m_log.Debug("[PHYSICS]:SetShape!"); + + if (Body != null) + { + DisposeOfBody(); + } + + if (prim_geom != null) + { + prim_geom.Dispose(); + prim_geom = null; + } + prim_geom = shape; + + //Body.set + } + + public void SetBody(float mass) + { + m_log.DebugFormat("[PHYSICS]: SetBody! {0}",mass); + if (Body != null && Body.Handle != IntPtr.Zero) + { + DisposeOfBody(); + } + + if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero) + tempMotionState1.Dispose(); + if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero) + tempTransform2.Dispose(); + if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero) + tempOrientation2.Dispose(); + + if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero) + tempPosition2.Dispose(); + + tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W); + tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z); + tempTransform2 = new btTransform(tempOrientation2, tempPosition2); + tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero); + if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero) + tempInertia1.Dispose(); + tempInertia1 = new btVector3(0, 0, 0); + if (prim_geom.Handle == IntPtr.Zero) + { + m_log.Warn("[PHYSICS]:PrimGeom is Disposed!"); + CreateGeom(IntPtr.Zero, primMesh); + + } + prim_geom.calculateLocalInertia(mass, tempInertia1); + + if (mass == 0) + Body = new btRigidBody(mass, tempMotionState1, prim_geom); + else + Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1); + + if (prim_geom is btGImpactMeshShape) + { + ((btGImpactMeshShape) prim_geom).setLocalScaling(new btVector3(1, 1, 1)); + ((btGImpactMeshShape) prim_geom).updateBound(); + } + _parent_scene.AddPrimToScene(this); + } + + private void DisposeOfBody() + { + if (Body != null) + { + if (Body.Handle != IntPtr.Zero) + { + _parent_scene.removeFromWorld(this,Body); + Body.Dispose(); + } + Body = null; + // TODO: dispose parts that make up body + } + } + + private void ChildDelink(BulletDotNETPrim pPrim) + { + // Okay, we have a delinked child.. need to rebuild the body. + lock (childrenPrim) + { + foreach (BulletDotNETPrim prm in childrenPrim) + { + prm.childPrim = true; + prm.disableBody(); + + } + } + disableBody(); + + lock (childrenPrim) + { + childrenPrim.Remove(pPrim); + } + + + + + if (Body != null && Body.Handle != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + } + + + + lock (childrenPrim) + { + foreach (BulletDotNETPrim prm in childrenPrim) + { + ParentPrim(prm); + } + } + + } + + private void ParentPrim(BulletDotNETPrim prm) + { + // TODO: Parent Linking algorithm. Use btComplexObject + } + + public void disableBody() + { + //this kills the body so things like 'mesh' can re-create it. + lock (this) + { + if (!childPrim) + { + if (Body != null && Body.Handle != IntPtr.Zero) + { + _parent_scene.remActivePrim(this); + + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + if (prim_geom != null && prim_geom.Handle != IntPtr.Zero) + { + // TODO: Set Category bits and Flags + } + + // TODO: destroy body + DisposeOfBody(); + + lock (childrenPrim) + { + if (childrenPrim.Count > 0) + { + foreach (BulletDotNETPrim prm in childrenPrim) + { + _parent_scene.remActivePrim(prm); + prm.DisposeOfBody(); + prm.SetCollisionShape(null); + } + } + + } + + DisposeOfBody(); + } + } + else + { + _parent_scene.remActivePrim(this); + m_collisionCategories &= ~CollisionCategories.Body; + m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); + + if (prim_geom != null && prim_geom.Handle != IntPtr.Zero) + { + // TODO: Set Category bits and Flags + } + + DisposeOfBody(); + } + + } + m_disabled = true; + m_collisionscore = 0; + } + + public void disableBodySoft() + { + m_disabled = true; + + if (m_isphysical && Body.Handle != IntPtr.Zero) + { + Body.clearForces(); + Body.forceActivationState(0); + + } + + } + + public void enableBodySoft() + { + if (!childPrim) + { + if (m_isphysical && Body.Handle != IntPtr.Zero) + { + Body.clearForces(); + Body.forceActivationState(1); + } + m_disabled = false; + } + } + + public void enableBody() + { + if (!childPrim) + { + //SetCollisionShape(prim_geom); + if (IsPhysical) + SetBody(Mass); + else + SetBody(0); + + // TODO: Set Collision Category Bits and Flags + // TODO: Set Auto Disable data + + m_interpenetrationcount = 0; + m_collisionscore = 0; + m_disabled = false; + // The body doesn't already have a finite rotation mode set here + if ((!m_angularlock.IsIdentical(PhysicsVector.Zero, 0)) && _parent == null) + { + // TODO: Create Angular Motor on Axis Lock! + } + _parent_scene.addActivePrim(this); + } + } + + public void UpdatePositionAndVelocity() + { + if (_parent == null) + { + PhysicsVector pv = new PhysicsVector(0, 0, 0); + bool lastZeroFlag = _zeroFlag; + if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero) + tempPosition2.Dispose(); + if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero) + tempTransform3.Dispose(); + + if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero) + tempOrientation2.Dispose(); + + if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero) + tempAngularVelocity1.Dispose(); + + if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero) + tempLinearVelocity1.Dispose(); + + + + tempTransform3 = Body.getInterpolationWorldTransform(); + tempPosition2 = tempTransform3.getOrigin(); // vec + tempOrientation2 = tempTransform3.getRotation(); // ori + tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel + tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel + + _torque.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(), tempAngularVelocity1.getZ()); + PhysicsVector l_position = new PhysicsVector(); + Quaternion l_orientation = new Quaternion(); + m_lastposition = _position; + m_lastorientation = _orientation; + + l_position.X = tempPosition2.getX(); + l_position.Y = tempPosition2.getY(); + l_position.Z = tempPosition2.getZ(); + l_orientation.X = tempOrientation2.getX(); + l_orientation.Y = tempOrientation2.getY(); + l_orientation.Z = tempOrientation2.getZ(); + l_orientation.W = tempOrientation2.getW(); + + if (l_position.X > 255.95f || l_position.X < 0f || l_position.Y > 255.95f || l_position.Y < 0f) + { + //base.RaiseOutOfBounds(l_position); + + if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) + { + _position = l_position; + //_parent_scene.remActivePrim(this); + if (_parent == null) + base.RequestPhysicsterseUpdate(); + return; + } + else + { + if (_parent == null) + base.RaiseOutOfBounds(l_position); + return; + } + } + + if (l_position.Z < -200000f) + { + // This is so prim that get lost underground don't fall forever and suck up + // + // Sim resources and memory. + // Disables the prim's movement physics.... + // It's a hack and will generate a console message if it fails. + + //IsPhysical = false; + //if (_parent == null) + //base.RaiseOutOfBounds(_position); + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + _velocity.X = 0; + _velocity.Y = 0; + _velocity.Z = 0; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + + if (_parent == null) + base.RequestPhysicsterseUpdate(); + + m_throttleUpdates = false; + throttleCounter = 0; + _zeroFlag = true; + //outofBounds = true; + } + + if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) + && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) + && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) + && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01 )) + { + _zeroFlag = true; + m_throttleUpdates = false; + } + else + { + //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); + _zeroFlag = false; + } + + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + + _acceleration.X = 0; + _acceleration.Y = 0; + _acceleration.Z = 0; + + //_orientation.w = 0f; + //_orientation.X = 0f; + //_orientation.Y = 0f; + //_orientation.Z = 0f; + m_rotationalVelocity.X = 0; + m_rotationalVelocity.Y = 0; + m_rotationalVelocity.Z = 0; + if (!m_lastUpdateSent) + { + m_throttleUpdates = false; + throttleCounter = 0; + m_rotationalVelocity = pv; + + if (_parent == null) + base.RequestPhysicsterseUpdate(); + + m_lastUpdateSent = true; + } + } + else + { + if (lastZeroFlag != _zeroFlag) + { + if (_parent == null) + base.RequestPhysicsterseUpdate(); + } + + m_lastVelocity = _velocity; + + _position = l_position; + + _velocity.X = tempLinearVelocity1.getX(); + _velocity.Y = tempLinearVelocity1.getY(); + _velocity.Z = tempLinearVelocity1.getZ(); + + _acceleration = ((_velocity - m_lastVelocity) / 0.1f); + _acceleration = new PhysicsVector(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); + //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); + + if (_velocity.IsIdentical(pv, 0.5f)) + { + m_rotationalVelocity = pv; + } + else + { + + m_rotationalVelocity.setValues(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ()); + } + + //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); + + _orientation.X = l_orientation.X; + _orientation.Y = l_orientation.Y; + _orientation.Z = l_orientation.Z; + _orientation.W = l_orientation.W; + m_lastUpdateSent = false; + + //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) + //{ + if (_parent == null) + base.RequestPhysicsterseUpdate(); + // } + // else + // { + // throttleCounter++; + //} + + } + m_lastposition = l_position; + } + else + { + // 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; + } + } + + + internal void setPrimForRemoval() + { + m_taintremove = true; + } + } +} + diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs new file mode 100644 index 0000000..4fcf035 --- /dev/null +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETScene.cs @@ -0,0 +1,642 @@ +using System; +using System.Collections.Generic; +using System.Reflection; +using System.IO; +using System.Diagnostics; +using System.Threading; +using log4net; +using Nini.Config; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; +using BulletDotNET; + +namespace OpenSim.Region.Physics.BulletDotNETPlugin +{ + public class BulletDotNETScene : PhysicsScene + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + private string m_sceneIdentifier = string.Empty; + + private List m_characters = new List(); + private List m_prims = new List(); + private List m_activePrims = new List(); + private List m_taintedActors = new List(); + private btDiscreteDynamicsWorld m_world; + private btAxisSweep3 m_broadphase; + private btCollisionConfiguration m_collisionConfiguration; + private btConstraintSolver m_solver; + private btCollisionDispatcher m_dispatcher; + private btHeightfieldTerrainShape m_terrainShape; + public btRigidBody TerrainBody; + private btVector3 m_terrainPosition; + private btVector3 m_gravity; + public btMotionState m_terrainMotionState; + public btTransform m_terrainTransform; + public btVector3 VectorZero; + public btQuaternion QuatIdentity; + public btTransform TransZero; + + public float geomDefaultDensity = 10.000006836f; + + private float avPIDD = 65f; + private float avPIDP = 28f; + private float avCapRadius = 0.37f; + private float avStandupTensor = 2000000f; + private float avDensity = 80f; + private float avHeightFudgeFactor = 0.52f; + private float avMovementDivisorWalk = 1.0f; + private float avMovementDivisorRun = 0.75f; + + private float minimumGroundFlightOffset = 3f; + + public bool meshSculptedPrim = true; + + public float meshSculptLOD = 32; + public float MeshSculptphysicalLOD = 16; + + public float bodyPIDD = 35f; + public float bodyPIDG = 25; + internal int geomCrossingFailuresBeforeOutofbounds = 4; + + public float bodyMotorJointMaxforceTensor = 2; + + public int bodyFramesAutoDisable = 20; + + public float WorldTimeStep = 10f/60f; + public const float WorldTimeComp = 1/60f; + public float gravityz = -9.8f; + + private float[] _origheightmap; // Used for Fly height. Kitto Flora + private bool usingGImpactAlgorithm = false; + + private IConfigSource m_config; + private readonly btVector3 worldAabbMin = new btVector3(0, 0, 0); + private readonly btVector3 worldAabbMax = new btVector3(Constants.RegionSize, Constants.RegionSize , 9000); + + public IMesher mesher; + + public BulletDotNETScene(string sceneIdentifier) + { + m_sceneIdentifier = sceneIdentifier; + VectorZero = new btVector3(0, 0, 0); + QuatIdentity = new btQuaternion(0, 0, 0, 1); + TransZero = new btTransform(QuatIdentity, VectorZero); + m_gravity = new btVector3(0, 0, gravityz); + _origheightmap = new float[(int)Constants.RegionSize * (int)Constants.RegionSize]; + } + + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { + mesher = meshmerizer; + m_config = config; + if (Environment.OSVersion.Platform == PlatformID.Unix) + { + m_log.Fatal("[BulletDotNET]: This configuration is not supported on *nix currently"); + Thread.Sleep(5000); + Environment.Exit(0); + } + m_broadphase = new btAxisSweep3(worldAabbMin, worldAabbMax, 16000); + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + m_solver = new btSequentialImpulseConstraintSolver(); + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_world = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_world.setGravity(m_gravity); + } + + public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size, bool isFlying) + { + BulletDotNETCharacter chr = new BulletDotNETCharacter(avName, this, position, size, avPIDD, avPIDP, + avCapRadius, avStandupTensor, avDensity, + avHeightFudgeFactor, avMovementDivisorWalk, + avMovementDivisorRun); + m_characters.Add(chr); + AddPhysicsActorTaint(chr); + return chr; + } + + public override void RemoveAvatar(PhysicsActor actor) + { + BulletDotNETCharacter chr = (BulletDotNETCharacter) actor; + + m_characters.Remove(chr); + m_world.removeRigidBody(chr.Body); + m_world.removeCollisionObject(chr.Body); + + chr.Remove(); + AddPhysicsActorTaint(chr); + //chr = null; + } + + public override void RemovePrim(PhysicsActor prim) + { + if (prim is BulletDotNETPrim) + { + + BulletDotNETPrim p = (BulletDotNETPrim)prim; + + p.setPrimForRemoval(); + AddPhysicsActorTaint(prim); + //RemovePrimThreadLocked(p); + + } + } + + private PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, Quaternion rotation, + IMesh mesh, PrimitiveBaseShape pbs, bool isphysical) + { + PhysicsVector pos = new PhysicsVector(position.X, position.Y, position.Z); + //pos.X = position.X; + //pos.Y = position.Y; + //pos.Z = position.Z; + PhysicsVector siz = new PhysicsVector(); + siz.X = size.X; + siz.Y = size.Y; + siz.Z = size.Z; + Quaternion rot = rotation; + + BulletDotNETPrim newPrim; + + newPrim = new BulletDotNETPrim(name, this, pos, siz, rot, mesh, pbs, isphysical); + + lock (m_prims) + m_prims.Add(newPrim); + + + return newPrim; + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, Quaternion rotation) + { + return AddPrimShape(primName, pbs, position, size, rotation, false); + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position, PhysicsVector size, Quaternion rotation, bool isPhysical) + { + PhysicsActor result; + IMesh mesh = null; + + //switch (pbs.ProfileShape) + //{ + // case ProfileShape.Square: + // //support simple box & hollow box now; later, more shapes + // if (needsMeshing(pbs)) + // { + // mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); + // } + + // break; + //} + + if (needsMeshing(pbs)) + mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical); + + result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical); + + return result; + } + + public override void AddPhysicsActorTaint(PhysicsActor prim) + { + lock (m_taintedActors) + { + if (!m_taintedActors.Contains(prim)) + { + m_taintedActors.Add(prim); + } + } + } + internal void SetUsingGImpact() + { + if (!usingGImpactAlgorithm) + btGImpactCollisionAlgorithm.registerAlgorithm(m_dispatcher); + usingGImpactAlgorithm = true; + } + + public override float Simulate(float timeStep) + { + lock (m_taintedActors) + { + foreach (PhysicsActor act in m_taintedActors) + { + if (act is BulletDotNETCharacter) + ((BulletDotNETCharacter) act).ProcessTaints(timeStep); + if (act is BulletDotNETPrim) + ((BulletDotNETPrim)act).ProcessTaints(timeStep); + } + m_taintedActors.Clear(); + } + + lock (m_characters) + { + foreach (BulletDotNETCharacter chr in m_characters) + { + chr.Move(timeStep); + } + } + + lock (m_prims) + { + foreach (BulletDotNETPrim prim in m_prims) + { + prim.Move(timeStep); + } + } + float steps = m_world.stepSimulation(WorldTimeStep, 5, WorldTimeComp); + + foreach (BulletDotNETCharacter chr in m_characters) + { + chr.UpdatePositionAndVelocity(); + } + + foreach (BulletDotNETPrim prm in m_activePrims) + { + prm.UpdatePositionAndVelocity(); + } + + return steps; + } + + public override void GetResults() + { + + } + + public override void SetTerrain(float[] heightMap) + { + if (m_terrainShape != null) + DeleteTerrain(); + + float hfmax = -9000; + float hfmin = 90000; + + for (int i = 0; i hfmax) ? heightMap[i] : hfmax; + } + // store this for later reference. + // Note, we're storing it after we check it for anomolies above + _origheightmap = heightMap; + + hfmin = 0; + hfmax = 256; + + m_terrainShape = new btHeightfieldTerrainShape((int)Constants.RegionSize, (int)Constants.RegionSize, heightMap, + 1.0f, hfmin, hfmax, (int)btHeightfieldTerrainShape.UPAxis.Z, + (int)btHeightfieldTerrainShape.PHY_ScalarType.PHY_FLOAT, false); + float AabbCenterX = Constants.RegionSize/2f; + float AabbCenterY = Constants.RegionSize/2f; + + float AabbCenterZ = 0; + float temphfmin, temphfmax; + + temphfmin = hfmin; + temphfmax = hfmax; + + if (temphfmin < 0) + { + temphfmax = 0 - temphfmin; + temphfmin = 0 - temphfmin; + } + else if (temphfmin > 0) + { + temphfmax = temphfmax + (0 - temphfmin); + //temphfmin = temphfmin + (0 - temphfmin); + } + AabbCenterZ = temphfmax/2f; + + if (m_terrainPosition == null) + { + m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ); + } + else + { + try + { + m_terrainPosition.setValue(AabbCenterX, AabbCenterY, AabbCenterZ); + } + catch (ObjectDisposedException) + { + m_terrainPosition = new btVector3(AabbCenterX, AabbCenterY, AabbCenterZ); + } + } + if (m_terrainMotionState != null) + { + m_terrainMotionState.Dispose(); + m_terrainMotionState = null; + } + m_terrainTransform = new btTransform(QuatIdentity, m_terrainPosition); + m_terrainMotionState = new btDefaultMotionState(m_terrainTransform); + TerrainBody = new btRigidBody(0, m_terrainMotionState, m_terrainShape); + m_world.addRigidBody(TerrainBody); + + + } + + public override void SetWaterLevel(float baseheight) + { + + } + + public override void DeleteTerrain() + { + if (TerrainBody != null) + { + m_world.removeRigidBody(TerrainBody); + } + + if (m_terrainShape != null) + { + m_terrainShape.Dispose(); + m_terrainShape = null; + } + + if (m_terrainMotionState != null) + { + m_terrainMotionState.Dispose(); + m_terrainMotionState = null; + } + + if (m_terrainTransform != null) + { + m_terrainTransform.Dispose(); + m_terrainTransform = null; + } + + if (m_terrainPosition != null) + { + m_terrainPosition.Dispose(); + m_terrainPosition = null; + } + } + + public override void Dispose() + { + disposeAllBodies(); + m_world.Dispose(); + m_broadphase.Dispose(); + ((btDefaultCollisionConfiguration) m_collisionConfiguration).Dispose(); + ((btSequentialImpulseConstraintSolver) m_solver).Dispose(); + worldAabbMax.Dispose(); + worldAabbMin.Dispose(); + VectorZero.Dispose(); + QuatIdentity.Dispose(); + m_gravity.Dispose(); + VectorZero = null; + QuatIdentity = null; + } + + public override Dictionary GetTopColliders() + { + return new Dictionary(); + } + + public btDiscreteDynamicsWorld getBulletWorld() + { + return m_world; + } + + private void disposeAllBodies() + { + lock (m_prims) + { + foreach ( BulletDotNETPrim prim in m_prims) + { + if (prim.Body != null) + m_world.removeRigidBody(prim.Body); + + prim.Dispose(); + } + m_prims.Clear(); + + foreach (BulletDotNETCharacter chr in m_characters) + { + if (chr.Body != null) + m_world.removeRigidBody(chr.Body); + chr.Dispose(); + } + m_characters.Clear(); + } + } + + public override bool IsThreaded + { + get { return false; } + } + + internal void addCollisionEventReporting(PhysicsActor bulletDotNETCharacter) + { + //TODO: FIXME: + } + + internal void remCollisionEventReporting(PhysicsActor bulletDotNETCharacter) + { + //TODO: FIXME: + } + + internal void AddRigidBody(btRigidBody Body) + { + m_world.addRigidBody(Body); + } + [Obsolete("bad!")] + internal void removeFromWorld(btRigidBody body) + { + + m_world.removeRigidBody(body); + } + + internal void removeFromWorld(BulletDotNETPrim prm ,btRigidBody body) + { + lock (m_prims) + { + if (m_prims.Contains(prm)) + { + m_world.removeRigidBody(body); + } + m_prims.Remove(prm); + } + + } + + internal float GetWaterLevel() + { + throw new NotImplementedException(); + } + + // Recovered for use by fly height. Kitto Flora + public float GetTerrainHeightAtXY(float x, float y) + { + // Teravus: Kitto, this code causes recurring errors that stall physics permenantly unless + // the values are checked, so checking below. + // Is there any reason that we don't do this in ScenePresence? + // The only physics engine that benefits from it in the physics plugin is this one + + if ((int)x > Constants.RegionSize || (int)y > Constants.RegionSize || + (int)x < 0.001f || (int)y < 0.001f) + return 0; + + return _origheightmap[(int)y * Constants.RegionSize + (int)x]; + } + // End recovered. Kitto Flora + + /// + /// 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 && !meshSculptedPrim) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + 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 ((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; + } + } + + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + + + if (iPropertiesNotSupportedDefault == 0) + { +#if SPAM + m_log.Warn("NonMesh"); +#endif + return false; + } +#if SPAM + m_log.Debug("Mesh"); +#endif + return true; + } + + internal void addActivePrim(BulletDotNETPrim pPrim) + { + lock (m_activePrims) + { + if (!m_activePrims.Contains(pPrim)) + { + m_activePrims.Add(pPrim); + } + } + } + + public void remActivePrim(BulletDotNETPrim pDeactivatePrim) + { + lock (m_activePrims) + { + m_activePrims.Remove(pDeactivatePrim); + } + } + + internal void AddPrimToScene(BulletDotNETPrim pPrim) + { + lock (m_prims) + { + if (!m_prims.Contains(pPrim)) + { + m_prims.Add(pPrim); + m_world.addRigidBody(pPrim.Body); + } + } + } + } +} -- cgit v1.1