diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs new file mode 100644 index 0000000000..d9e236f2c4 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs @@ -0,0 +1,426 @@ +/* + * 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 copyrightD + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +using System; +using System.Collections.Generic; +using System.Reflection; +using log4net; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.BulletSPlugin +{ +public class BSCharacter : PhysicsActor +{ + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private static readonly string LogHeader = "[BULLETS CHAR]"; + + private BSScene _scene; + private String _avName; + private bool _stopped; + private Vector3 _size; + private Vector3 _scale; + private PrimitiveBaseShape _pbs; + private uint _localID = 0; + private bool _grabbed; + private bool _selected; + private Vector3 _position; + private float _mass = 80f; + public float _density = 60f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + private Vector3 _force; + private Vector3 _velocity; + private Vector3 _torque; + private float _collisionScore; + private Vector3 _acceleration; + private Quaternion _orientation; + private int _physicsActorType; + private bool _isPhysical; + private bool _flying; + private bool _setAlwaysRun; + private bool _throttleUpdates; + private bool _isColliding; + private long _collidingStep; + private bool _collidingGround; + private long _collidingGroundStep; + private bool _collidingObj; + private bool _floatOnWater; + private Vector3 _rotationalVelocity; + private bool _kinematic; + private float _buoyancy; + + private int _subscribedEventsMs = 0; + private int _lastCollisionTime = 0; + + private Vector3 _PIDTarget; + private bool _usePID; + private float _PIDTau; + private bool _useHoverPID; + private float _PIDHoverHeight; + private PIDHoverType _PIDHoverType; + private float _PIDHoverTao; + + public BSCharacter(uint localID, String avName, BSScene parent_scene, Vector3 pos, Vector3 size, bool isFlying) + { + _localID = localID; + _avName = avName; + _scene = parent_scene; + _position = pos; + _size = size; + _orientation = Quaternion.Identity; + _velocity = Vector3.Zero; + _buoyancy = 0f; // characters return a buoyancy of zero + _scale = new Vector3(1f, 1f, 1f); + float AVvolume = (float) (Math.PI*Math.Pow(CAPSULE_RADIUS, 2)*CAPSULE_LENGTH); + _mass = _density*AVvolume; + + ShapeData shapeData = new ShapeData(); + shapeData.ID = _localID; + shapeData.Type = ShapeData.PhysicsShapeType.SHAPE_AVATAR; + shapeData.Position = _position; + shapeData.Rotation = _orientation; + shapeData.Velocity = _velocity; + shapeData.Scale = _scale; + shapeData.Mass = _mass; + shapeData.Buoyancy = isFlying ? 0f : 1f; + shapeData.Static = ShapeData.numericFalse; + + // do actual create at taint time + _scene.TaintedObject(delegate() + { + BulletSimAPI.CreateObject(parent_scene.WorldID, shapeData); + }); + + return; + } + + // called when this character is being destroyed and the resources should be released + public void Destroy() + { + _scene.TaintedObject(delegate() + { + BulletSimAPI.DestroyObject(_scene.WorldID, _localID); + }); + } + + public override void RequestPhysicsterseUpdate() + { + base.RequestPhysicsterseUpdate(); + } + + public override bool Stopped { + get { return _stopped; } + } + public override Vector3 Size { + get { return _size; } + set { _size = value; + } + } + public override PrimitiveBaseShape Shape { + set { _pbs = value; + } + } + public override uint LocalID { + set { _localID = value; + } + get { return _localID; } + } + public override bool Grabbed { + set { _grabbed = value; + } + } + public override bool Selected { + set { _selected = value; + } + } + public override void CrossingFailure() { return; } + public override void link(PhysicsActor obj) { return; } + public override void delink() { return; } + public override void LockAngularMotion(Vector3 axis) { return; } + + public override Vector3 Position { + get { + // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); + return _position; + } + set { + _position = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); + }); + } + } + public override float Mass { + get { + return _mass; + } + } + public override Vector3 Force { + get { return _force; } + set { + _force = value; + m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force); + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); + }); + } + } + + public override int VehicleType { + get { return 0; } + set { return; } + } + public override void VehicleFloatParam(int param, float value) { } + public override void VehicleVectorParam(int param, Vector3 value) {} + public override void VehicleRotationParam(int param, Quaternion rotation) { } + public override void VehicleFlags(int param, bool remove) { } + + // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more + public override void SetVolumeDetect(int param) { return; } + + public override Vector3 GeometricCenter { get { return Vector3.Zero; } } + public override Vector3 CenterOfMass { get { return Vector3.Zero; } } + public override Vector3 Velocity { + get { return _velocity; } + set { + _velocity = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectVelocity(_scene.WorldID, _localID, _velocity); + }); + } + } + public override Vector3 Torque { + get { return _torque; } + set { _torque = value; + } + } + public override float CollisionScore { + get { return _collisionScore; } + set { _collisionScore = value; + } + } + public override Vector3 Acceleration { + get { return _acceleration; } + } + public override Quaternion Orientation { + get { return _orientation; } + set { + _orientation = value; + _scene.TaintedObject(delegate() + { + // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); + BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); + }); + } + } + public override int PhysicsActorType { + get { return _physicsActorType; } + set { _physicsActorType = value; + } + } + public override bool IsPhysical { + get { return _isPhysical; } + set { _isPhysical = value; + } + } + public override bool Flying { + get { return _flying; } + set { + _flying = value; + _scene.TaintedObject(delegate() + { + // simulate flying by changing the effect of gravity + BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _flying ? 0f : 1f); + }); + } + } + public override bool + SetAlwaysRun { + get { return _setAlwaysRun; } + set { _setAlwaysRun = value; } + } + public override bool ThrottleUpdates { + get { return _throttleUpdates; } + set { _throttleUpdates = value; } + } + public override bool IsColliding { + get { return (_collidingStep == _scene.SimulationStep); } + set { _isColliding = value; } + } + public override bool CollidingGround { + get { return (_collidingGroundStep == _scene.SimulationStep); } + set { _collidingGround = value; } + } + public override bool CollidingObj { + get { return _collidingObj; } + set { _collidingObj = value; } + } + public override bool FloatOnWater { + set { _floatOnWater = value; } + } + public override Vector3 RotationalVelocity { + get { return _rotationalVelocity; } + set { _rotationalVelocity = value; } + } + public override bool Kinematic { + get { return _kinematic; } + set { _kinematic = value; } + } + public override float Buoyancy { + get { return _buoyancy; } + set { _buoyancy = value; } + } + + // Used for MoveTo + public override Vector3 PIDTarget { + set { _PIDTarget = value; } + } + public override bool PIDActive { + set { _usePID = value; } + } + public override float PIDTau { + set { _PIDTau = value; } + } + + // Used for llSetHoverHeight and maybe vehicle height + // Hover Height will override MoveTo target's Z + public override bool PIDHoverActive { + set { _useHoverPID = value; } + } + public override float PIDHoverHeight { + set { _PIDHoverHeight = value; } + } + public override PIDHoverType PIDHoverType { + set { _PIDHoverType = value; } + } + public override float PIDHoverTau { + set { _PIDHoverTao = value; } + } + + // For RotLookAt + public override Quaternion APIDTarget { set { return; } } + public override bool APIDActive { set { return; } } + public override float APIDStrength { set { return; } } + public override float APIDDamping { set { return; } } + + public override void AddForce(Vector3 force, bool pushforce) { + if (force.IsFinite()) + { + _force.X += force.X; + _force.Y += force.Y; + _force.Z += force.Z; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); + }); + } + else + { + m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader); + } + //m_lastUpdateSent = false; + } + public override void AddAngularForce(Vector3 force, bool pushforce) { + } + public override void SetMomentum(Vector3 momentum) { + } + public override void SubscribeEvents(int ms) { + _subscribedEventsMs = ms; + _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen + } + public override void UnSubscribeEvents() { + _subscribedEventsMs = 0; + } + public override bool SubscribedEvents() { + return (_subscribedEventsMs > 0); + } + + // The physics engine says that properties have updated. Update same and inform + // the world that things have changed. + public void UpdateProperties(EntityProperties entprop) + { + bool changed = false; + // we assign to the local variables so the normal set action does not happen + if (_position != entprop.Position) + { + _position = entprop.Position; + changed = true; + } + if (_orientation != entprop.Rotation) + { + _orientation = entprop.Rotation; + changed = true; + } + if (_velocity != entprop.Velocity) + { + _velocity = entprop.Velocity; + changed = true; + } + if (_acceleration != entprop.Acceleration) + { + _acceleration = entprop.Acceleration; + changed = true; + } + if (_rotationalVelocity != entprop.AngularVelocity) + { + _rotationalVelocity = entprop.AngularVelocity; + changed = true; + } + if (changed) + { + // base.RequestPhysicsterseUpdate(); + } + } + + public void Collide(uint collidingWith, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth) + { + // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith); + + // The following makes IsColliding() and IsCollidingGround() work + _collidingStep = _scene.SimulationStep; + if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID) + { + _collidingGroundStep = _scene.SimulationStep; + } + + if (_subscribedEventsMs == 0) return; // don't want collisions + int nowTime = Util.EnvironmentTickCount(); + if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return; + _lastCollisionTime = nowTime; + + Dictionary contactPoints = new Dictionary(); + contactPoints.Add(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); + CollisionEventUpdate args = new CollisionEventUpdate(LocalID, (int)type, 1, contactPoints); + base.SendCollisionUpdate(args); + } + +} +} diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs new file mode 100644 index 0000000000..046726d979 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs @@ -0,0 +1,951 @@ +/* + * 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. + */ + +/* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to + * call the BulletSim system. + */ +/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces + * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: + * ODEPrim.cs contains methods dealing with Prim editing, Prim + * characteristics and Kinetic motion. + * ODEDynamics.cs contains methods dealing with Prim Physical motion + * (dynamics) and the associated settings. Old Linear and angular + * motors for dynamic motion have been replace with MoveLinear() + * and MoveAngular(); 'Physical' is used only to switch ODE dynamic + * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_ is to + * switch between 'VEHICLE' parameter use and general dynamics + * settings use. + */ + +using System; +using System.Collections.Generic; +using System.Reflection; +using System.Runtime.InteropServices; +using log4net; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.BulletSPlugin +{ + public class BSDynamics + { + private int frcount = 0; // Used to limit dynamics debug output to + // every 100th frame + + // private BSScene m_parentScene = null; + private BSPrim m_prim; // the prim this dynamic controller belongs to + + // Vehicle properties + private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind + public Vehicle Type + { + get { return m_type; } + } + // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier + private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: + // HOVER_TERRAIN_ONLY + // HOVER_GLOBAL_HEIGHT + // NO_DEFLECTION_UP + // HOVER_WATER_ONLY + // HOVER_UP_ONLY + // LIMIT_MOTOR_UP + // LIMIT_ROLL_ONLY + private VehicleFlag m_Hoverflags = (VehicleFlag)0; + private Vector3 m_BlockingEndPoint = Vector3.Zero; + private Quaternion m_RollreferenceFrame = Quaternion.Identity; + // Linear properties + private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time + private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL + private Vector3 m_dir = Vector3.Zero; // velocity applied to body + private Vector3 m_linearFrictionTimescale = Vector3.Zero; + private float m_linearMotorDecayTimescale = 0; + private float m_linearMotorTimescale = 0; + private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + private Vector3 m_lastPositionVector = Vector3.Zero; + // private bool m_LinearMotorSetLastFrame = false; + // private Vector3 m_linearMotorOffset = Vector3.Zero; + + //Angular properties + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body + + //Deflection properties + // private float m_angularDeflectionEfficiency = 0; + // private float m_angularDeflectionTimescale = 0; + // private float m_linearDeflectionEfficiency = 0; + // private float m_linearDeflectionTimescale = 0; + + //Banking properties + // private float m_bankingEfficiency = 0; + // private float m_bankingMix = 0; + // private float m_bankingTimescale = 0; + + //Hover and Buoyancy properties + private float m_VhoverHeight = 0f; +// private float m_VhoverEfficiency = 0f; + private float m_VhoverTimescale = 0f; + private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height + private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. + // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) + // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. + // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. + + //Attractor properties + private float m_verticalAttractionEfficiency = 1.0f; // damped + private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. + + public BSDynamics(BSPrim myPrim) + { + m_prim = myPrim; + m_type = Vehicle.TYPE_NONE; + } + + internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionEfficiency = pValue; + break; + case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_angularDeflectionTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorDecayTimescale = pValue; + break; + case Vehicle.ANGULAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_angularMotorTimescale = pValue; + break; + case Vehicle.BANKING_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingEfficiency = pValue; + break; + case Vehicle.BANKING_MIX: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingMix = pValue; + break; + case Vehicle.BANKING_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_bankingTimescale = pValue; + break; + case Vehicle.BUOYANCY: + if (pValue < -1f) pValue = -1f; + if (pValue > 1f) pValue = 1f; + m_VehicleBuoyancy = pValue; + break; +// case Vehicle.HOVER_EFFICIENCY: +// if (pValue < 0f) pValue = 0f; +// if (pValue > 1f) pValue = 1f; +// m_VhoverEfficiency = pValue; +// break; + case Vehicle.HOVER_HEIGHT: + m_VhoverHeight = pValue; + break; + case Vehicle.HOVER_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_VhoverTimescale = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionEfficiency = pValue; + break; + case Vehicle.LINEAR_DEFLECTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + // m_linearDeflectionTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorDecayTimescale = pValue; + break; + case Vehicle.LINEAR_MOTOR_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_linearMotorTimescale = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: + if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable + if (pValue > 1.0f) pValue = 1.0f; + m_verticalAttractionEfficiency = pValue; + break; + case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: + if (pValue < 0.01f) pValue = 0.01f; + m_verticalAttractionTimescale = pValue; + break; + + // These are vector properties but the engine lets you use a single float value to + // set all of the components to the same value + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue, pValue, pValue); + m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); + break; + + } + }//end ProcessFloatVehicleParam + + internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) + { + switch (pParam) + { + case Vehicle.ANGULAR_FRICTION_TIMESCALE: + m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.ANGULAR_MOTOR_DIRECTION: + m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + // Limit requested angular speed to 2 rps= 4 pi rads/sec + if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; + if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; + if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; + if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; + if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; + if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; + m_angularMotorApply = 10; + break; + case Vehicle.LINEAR_FRICTION_TIMESCALE: + m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_DIRECTION: + m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); + m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.LINEAR_MOTOR_OFFSET: + // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + case Vehicle.BLOCK_EXIT: + m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); + break; + } + }//end ProcessVectorVehicleParam + + internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) + { + switch (pParam) + { + case Vehicle.REFERENCE_FRAME: + // m_referenceFrame = pValue; + break; + case Vehicle.ROLL_FRAME: + m_RollreferenceFrame = pValue; + break; + } + }//end ProcessRotationVehicleParam + + internal void ProcessVehicleFlags(int pParam, bool remove) + { + if (remove) + { + if (pParam == -1) + { + m_flags = (VehicleFlag)0; + m_Hoverflags = (VehicleFlag)0; + return; + } + if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0) + m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT); + } + if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY) + { + if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0) + m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY); + } + if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY) + { + if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0) + m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY); + } + if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY) + { + if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0) + m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY); + } + if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP) + { + if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP); + } + if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY) + { + if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY); + } + if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK) + { + if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.MOUSELOOK_BANK); + } + if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER) + { + if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.MOUSELOOK_STEER); + } + if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP) + { + if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP); + } + if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED) + { + if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED); + } + if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X) + { + if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.NO_X); + } + if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y) + { + if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.NO_Y); + } + if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z) + { + if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.NO_Z); + } + if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT) + { + if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0) + m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT); + } + if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION) + { + if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.NO_DEFLECTION); + } + if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION) + { + if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0) + m_flags &= ~(VehicleFlag.LOCK_ROTATION); + } + } + else + { + if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) + { + m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags); + } + if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY) + { + m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags); + } + if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY) + { + m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags); + } + if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY) + { + m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags); + } + if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP) + { + m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags); + } + if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK) + { + m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags); + } + if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER) + { + m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags); + } + if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP) + { + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags); + } + if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED) + { + m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags); + } + if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X) + { + m_flags |= (VehicleFlag.NO_X); + } + if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y) + { + m_flags |= (VehicleFlag.NO_Y); + } + if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z) + { + m_flags |= (VehicleFlag.NO_Z); + } + if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT) + { + m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT); + } + if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION) + { + m_flags |= (VehicleFlag.NO_DEFLECTION); + } + if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION) + { + m_flags |= (VehicleFlag.LOCK_ROTATION); + } + } + }//end ProcessVehicleFlags + + internal void ProcessTypeChange(Vehicle pType) + { + // Set Defaults For Type + m_type = pType; + switch (pType) + { + case Vehicle.TYPE_NONE: + m_linearFrictionTimescale = new Vector3(0, 0, 0); + m_angularFrictionTimescale = new Vector3(0, 0, 0); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 0; + m_linearMotorDecayTimescale = 0; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 0; + m_angularMotorDecayTimescale = 0; + m_VhoverHeight = 0; + m_VhoverTimescale = 0; + m_VehicleBuoyancy = 0; + m_flags = (VehicleFlag)0; + break; + + case Vehicle.TYPE_SLED: + m_linearFrictionTimescale = new Vector3(30, 1, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1000; + m_linearMotorDecayTimescale = 120; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1000; + m_angularMotorDecayTimescale = 120; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 1; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 1; + // m_linearDeflectionTimescale = 1; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 1000; + // m_bankingEfficiency = 0; + // m_bankingMix = 1; + // m_bankingTimescale = 10; + // m_referenceFrame = Quaternion.Identity; + m_Hoverflags &= + ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); + break; + case Vehicle.TYPE_CAR: + m_linearFrictionTimescale = new Vector3(100, 2, 1000); + m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 1; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 1; + m_angularMotorDecayTimescale = 0.8f; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // // m_linearDeflectionEfficiency = 1; + // // m_linearDeflectionTimescale = 2; + // // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 10; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 10f; + // m_bankingEfficiency = -0.2f; + // m_bankingMix = 1; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | + VehicleFlag.LIMIT_MOTOR_UP); + m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY); + break; + case Vehicle.TYPE_BOAT: + m_linearFrictionTimescale = new Vector3(10, 3, 2); + m_angularFrictionTimescale = new Vector3(10,10,10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 2; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 0.5f; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 0.5f; + m_verticalAttractionTimescale = 5f; + // m_bankingEfficiency = -0.3f; + // m_bankingMix = 0.8f; + // m_bankingTimescale = 1; + // m_referenceFrame = Quaternion.Identity; + m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY); + m_flags |= (VehicleFlag.NO_DEFLECTION_UP | + VehicleFlag.LIMIT_MOTOR_UP); + m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY); + break; + case Vehicle.TYPE_AIRPLANE: + m_linearFrictionTimescale = new Vector3(200, 10, 5); + m_angularFrictionTimescale = new Vector3(20, 20, 20); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 2; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 4; + m_angularMotorDecayTimescale = 4; + m_VhoverHeight = 0; +// m_VhoverEfficiency = 0.5f; + m_VhoverTimescale = 1000; + m_VehicleBuoyancy = 0; + // m_linearDeflectionEfficiency = 0.5f; + // m_linearDeflectionTimescale = 3; + // m_angularDeflectionEfficiency = 1; + // m_angularDeflectionTimescale = 2; + m_verticalAttractionEfficiency = 0.9f; + m_verticalAttractionTimescale = 2f; + // m_bankingEfficiency = 1; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 2; + // m_referenceFrame = Quaternion.Identity; + m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + break; + case Vehicle.TYPE_BALLOON: + m_linearFrictionTimescale = new Vector3(5, 5, 5); + m_angularFrictionTimescale = new Vector3(10, 10, 10); + m_linearMotorDirection = Vector3.Zero; + m_linearMotorTimescale = 5; + m_linearMotorDecayTimescale = 60; + m_angularMotorDirection = Vector3.Zero; + m_angularMotorTimescale = 6; + m_angularMotorDecayTimescale = 10; + m_VhoverHeight = 5; +// m_VhoverEfficiency = 0.8f; + m_VhoverTimescale = 10; + m_VehicleBuoyancy = 1; + // m_linearDeflectionEfficiency = 0; + // m_linearDeflectionTimescale = 5; + // m_angularDeflectionEfficiency = 0; + // m_angularDeflectionTimescale = 5; + m_verticalAttractionEfficiency = 1f; + m_verticalAttractionTimescale = 100f; + // m_bankingEfficiency = 0; + // m_bankingMix = 0.7f; + // m_bankingTimescale = 5; + // m_referenceFrame = Quaternion.Identity; + m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | + VehicleFlag.HOVER_UP_ONLY); + m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); + m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); + m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT); + break; + + } + }//end SetDefaultsForType + + internal void Step(float pTimestep, BSScene pParentScene) + { + if (m_type == Vehicle.TYPE_NONE) return; + + frcount++; // used to limit debug comment output + if (frcount > 100) + frcount = 0; + + MoveLinear(pTimestep, pParentScene); + MoveAngular(pTimestep); + LimitRotation(pTimestep); + }// end Step + + private void MoveLinear(float pTimestep, BSScene _pParentScene) + { + if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant + { + // add drive to body + Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); + m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? + + // This will work temporarily, but we really need to compare speed on an axis + // KF: Limit body velocity to applied velocity? + if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) + m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; + if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) + m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) + m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; + + // decay applied velocity + Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + else + { // requested is not significant + // if what remains of applied is small, zero it. + if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) + m_lastLinearVelocityVector = Vector3.Zero; + } + + // convert requested object velocity to world-referenced vector + m_dir = m_lastLinearVelocityVector; + Quaternion rot = m_prim.Orientation; + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object + m_dir *= rotq; // apply obj rotation to velocity vector + + // add Gravity andBuoyancy + // KF: So far I have found no good method to combine a script-requested + // .Z velocity and gravity. Therefore only 0g will used script-requested + // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. + Vector3 grav = Vector3.Zero; + // There is some gravity, make a gravity force vector + // that is applied after object velocity. + float objMass = m_prim.Mass; + // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; + grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy); + // Preserve the current Z velocity + Vector3 vel_now = m_prim.Velocity; + m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity + + Vector3 pos = m_prim.Position; +// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); + Vector3 posChange = new Vector3(); + posChange.X = pos.X - m_lastPositionVector.X; + posChange.Y = pos.Y - m_lastPositionVector.Y; + posChange.Z = pos.Z - m_lastPositionVector.Z; + double Zchange = Math.Abs(posChange.Z); + if (m_BlockingEndPoint != Vector3.Zero) + { + if (pos.X >= (m_BlockingEndPoint.X - (float)1)) + { + pos.X -= posChange.X + 1; + m_prim.Position = pos; + } + if (pos.Y >= (m_BlockingEndPoint.Y - (float)1)) + { + pos.Y -= posChange.Y + 1; + m_prim.Position = pos; + } + if (pos.Z >= (m_BlockingEndPoint.Z - (float)1)) + { + pos.Z -= posChange.Z + 1; + m_prim.Position = pos; + } + if (pos.X <= 0) + { + pos.X += posChange.X + 1; + m_prim.Position = pos; + } + if (pos.Y <= 0) + { + pos.Y += posChange.Y + 1; + m_prim.Position = pos; + } + } + if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y)) + { + pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2; + m_prim.Position = pos; + } + + // Check if hovering + if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) + { + // We should hover, get the target height + if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0) + { + m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; + } + if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) + { + m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; + } + if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) + { + m_VhoverTargetHeight = m_VhoverHeight; + } + + if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0) + { + // If body is aready heigher, use its height as target height + if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; + } + if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) + { + if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2) + { + m_prim.Position = pos; + } + } + else + { + float herr0 = pos.Z - m_VhoverTargetHeight; + // Replace Vertical speed with correction figure if significant + if (Math.Abs(herr0) > 0.01f) + { + m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale); + //KF: m_VhoverEfficiency is not yet implemented + } + else + { + m_dir.Z = 0f; + } + } + +// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped +// m_VhoverTimescale = 0f; // time to acheive height +// pTimestep is time since last frame,in secs + } + + if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) + { + //Start Experimental Values + if (Zchange > .3) + { + grav.Z = (float)(grav.Z * 3); + } + if (Zchange > .15) + { + grav.Z = (float)(grav.Z * 2); + } + if (Zchange > .75) + { + grav.Z = (float)(grav.Z * 1.5); + } + if (Zchange > .05) + { + grav.Z = (float)(grav.Z * 1.25); + } + if (Zchange > .025) + { + grav.Z = (float)(grav.Z * 1.125); + } + float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); + float postemp = (pos.Z - terraintemp); + if (postemp > 2.5f) + { + grav.Z = (float)(grav.Z * 1.037125); + } + //End Experimental Values + } + if ((m_flags & (VehicleFlag.NO_X)) != 0) + { + m_dir.X = 0; + } + if ((m_flags & (VehicleFlag.NO_Y)) != 0) + { + m_dir.Y = 0; + } + if ((m_flags & (VehicleFlag.NO_Z)) != 0) + { + m_dir.Z = 0; + } + + m_lastPositionVector = m_prim.Position; + + // Apply velocity + m_prim.Velocity = m_dir; + // apply gravity force + m_prim.Force = grav; + + + // apply friction + Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); + m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; + } // end MoveLinear() + + private void MoveAngular(float pTimestep) + { + /* + private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor + private int m_angularMotorApply = 0; // application frame counter + private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) + private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate + private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate + private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate + private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body + */ + + // Get what the body is doing, this includes 'external' influences + Vector3 angularVelocity = m_prim.AngularVelocity; + // Vector3 angularVelocity = Vector3.Zero; + + if (m_angularMotorApply > 0) + { + // ramp up to new value + // current velocity += error / (time to get there / step interval) + // requested speed - last motor speed + m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); + m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); + + m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected + // velocity may still be acheived. + } + else + { + // no motor recently applied, keep the body velocity + /* m_angularMotorVelocity.X = angularVelocity.X; + m_angularMotorVelocity.Y = angularVelocity.Y; + m_angularMotorVelocity.Z = angularVelocity.Z; */ + + // and decay the velocity + m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); + } // end motor section + + // Vertical attractor section + Vector3 vertattr = Vector3.Zero; + + if (m_verticalAttractionTimescale < 300) + { + float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); + // get present body rotation + Quaternion rotq = m_prim.Orientation; + // make a vector pointing up + Vector3 verterr = Vector3.Zero; + verterr.Z = 1.0f; + // rotate it to Body Angle + verterr = verterr * rotq; + // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. + // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go + // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. + if (verterr.Z < 0.0f) + { + verterr.X = 2.0f - verterr.X; + verterr.Y = 2.0f - verterr.Y; + } + // Error is 0 (no error) to +/- 2 (max error) + // scale it by VAservo + verterr = verterr * VAservo; +//if (frcount == 0) Console.WriteLine("VAerr=" + verterr); + + // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so + // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. + vertattr.X = verterr.Y; + vertattr.Y = - verterr.X; + vertattr.Z = 0f; + + // scaling appears better usingsquare-law + float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); + vertattr.X += bounce * angularVelocity.X; + vertattr.Y += bounce * angularVelocity.Y; + + } // else vertical attractor is off + + // m_lastVertAttractor = vertattr; + + // Bank section tba + // Deflection section tba + + // Sum velocities + m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection + + if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) + { + m_lastAngularVelocity.X = 0; + m_lastAngularVelocity.Y = 0; + } + + if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) + { + m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. + } + + // apply friction + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; + + // Apply to the body + m_prim.AngularVelocity = m_lastAngularVelocity; + + } //end MoveAngular + internal void LimitRotation(float timestep) + { + Quaternion rotq = m_prim.Orientation; // rotq = rotation of object + Quaternion m_rot = rotq; + bool changed = false; + if (m_RollreferenceFrame != Quaternion.Identity) + { + if (rotq.X >= m_RollreferenceFrame.X) + { + m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2); + } + if (rotq.Y >= m_RollreferenceFrame.Y) + { + m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2); + } + if (rotq.X <= -m_RollreferenceFrame.X) + { + m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2); + } + if (rotq.Y <= -m_RollreferenceFrame.Y) + { + m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2); + } + changed = true; + } + if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) + { + m_rot.X = 0; + m_rot.Y = 0; + changed = true; + } + if (changed) + m_prim.Orientation = m_rot; + } + } +} diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs new file mode 100644 index 0000000000..61be56d184 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs @@ -0,0 +1,68 @@ +/* + * 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 copyrightD + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +using System; +using System.Collections.Generic; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.BulletSPlugin +{ +public class BSPlugin : IPhysicsPlugin +{ + //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + + private BSScene _mScene; + + public BSPlugin() + { + } + + public bool Init() + { + return true; + } + + public PhysicsScene GetScene(String sceneIdentifier) + { + if (_mScene == null) + { + _mScene = new BSScene(sceneIdentifier); + } + return (_mScene); + } + + public string GetName() + { + return ("BulletSim"); + } + + public void Dispose() + { + } +} +} diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs new file mode 100644 index 0000000000..82a880377d --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs @@ -0,0 +1,1192 @@ +/* + * 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 copyrightD + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +using System; +using System.Reflection; +using System.Collections.Generic; +using System.Xml; +using log4net; +using OMV = OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenSim.Region.Physics.ConvexDecompositionDotNet; + +namespace OpenSim.Region.Physics.BulletSPlugin +{ + [Serializable] +public sealed class BSPrim : PhysicsActor +{ + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + private static readonly string LogHeader = "[BULLETS PRIM]"; + + private IMesh _mesh; + private PrimitiveBaseShape _pbs; + private ShapeData.PhysicsShapeType _shapeType; + private ulong _hullKey; + private List _hulls; + + private BSScene _scene; + private String _avName; + private uint _localID = 0; + + private OMV.Vector3 _size; + private OMV.Vector3 _scale; + private bool _stopped; + private bool _grabbed; + private bool _isSelected; + private bool _isVolumeDetect; + private OMV.Vector3 _position; + private float _mass; + private float _density; + private OMV.Vector3 _force; + private OMV.Vector3 _velocity; + private OMV.Vector3 _torque; + private float _collisionScore; + private OMV.Vector3 _acceleration; + private OMV.Quaternion _orientation; + private int _physicsActorType; + private bool _isPhysical; + private bool _flying; + private float _friction; + private bool _setAlwaysRun; + private bool _throttleUpdates; + private bool _isColliding; + private bool _collidingGround; + private bool _collidingObj; + private bool _floatOnWater; + private OMV.Vector3 _rotationalVelocity; + private bool _kinematic; + private float _buoyancy; + private OMV.Vector3 _angularVelocity; + + private List _childrenPrims; + private BSPrim _parentPrim; + + private int _subscribedEventsMs = 0; + private int _lastCollisionTime = 0; + long _collidingStep; + long _collidingGroundStep; + + private BSDynamics _vehicle; + + private OMV.Vector3 _PIDTarget; + private bool _usePID; + private float _PIDTau; + private bool _useHoverPID; + private float _PIDHoverHeight; + private PIDHoverType _PIDHoverType; + private float _PIDHoverTao; + + public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, + OMV.Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical) + { + // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID); + _localID = localID; + _avName = primName; + _scene = parent_scene; + _position = pos; + _size = size; + _scale = new OMV.Vector3(1f, 1f, 1f); // the scale will be set by CreateGeom depending on object type + _orientation = rotation; + _buoyancy = 1f; + _velocity = OMV.Vector3.Zero; + _angularVelocity = OMV.Vector3.Zero; + _mesh = mesh; + _hullKey = 0; + _pbs = pbs; + _isPhysical = pisPhysical; + _isVolumeDetect = false; + _subscribedEventsMs = 0; + _friction = _scene.DefaultFriction; // TODO: compute based on object material + _density = _scene.DefaultDensity; // TODO: compute based on object material + _parentPrim = null; // not a child or a parent + _vehicle = new BSDynamics(this); // add vehicleness + _childrenPrims = new List(); + if (_isPhysical) + _mass = CalculateMass(); + else + _mass = 0f; + // do the actual object creation at taint time + _scene.TaintedObject(delegate() + { + CreateGeom(); + CreateObject(); + }); + } + + // called when this prim is being destroyed and we should free all the resources + public void Destroy() + { + // m_log.DebugFormat("{0}: Destroy", LogHeader); + // Undo any vehicle properties + _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE); + _scene.RemoveVehiclePrim(this); // just to make sure + _scene.TaintedObject(delegate() + { + BulletSimAPI.DestroyObject(_scene.WorldID, _localID); + }); + } + + public override bool Stopped { + get { return _stopped; } + } + public override OMV.Vector3 Size { + get { return _size; } + set { + _size = value; + _scene.TaintedObject(delegate() + { + if (_isPhysical) _mass = CalculateMass(); // changing size changes the mass + BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, _mass, _isPhysical); + RecreateGeomAndObject(); + }); + } + } + public override PrimitiveBaseShape Shape { + set { + _pbs = value; + _scene.TaintedObject(delegate() + { + if (_isPhysical) _mass = CalculateMass(); // changing the shape changes the mass + RecreateGeomAndObject(); + }); + } + } + public override uint LocalID { + set { _localID = value; } + get { return _localID; } + } + public override bool Grabbed { + set { _grabbed = value; + } + } + public override bool Selected { + set { + _isSelected = value; + _scene.TaintedObject(delegate() + { + SetObjectDynamic(); + }); + } + } + public override void CrossingFailure() { return; } + + // link me to the specified parent + public override void link(PhysicsActor obj) { + BSPrim parent = (BSPrim)obj; + // m_log.DebugFormat("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID); + // TODO: decide if this parent checking needs to happen at taint time + if (_parentPrim == null) + { + if (parent != null) + { + // I don't have a parent so I am joining a linkset + parent.AddChildToLinkset(this); + } + } + else + { + // I already have a parent, is parenting changing? + if (parent != _parentPrim) + { + if (parent == null) + { + // we are being removed from a linkset + _parentPrim.RemoveChildFromLinkset(this); + } + else + { + // asking to reparent a prim should not happen + m_log.ErrorFormat("{0}: Reparenting a prim. ", LogHeader); + } + } + } + return; + } + + // delink me from my linkset + public override void delink() { + // TODO: decide if this parent checking needs to happen at taint time + // Race condition here: if link() and delink() in same simulation tick, the delink will not happen + // m_log.DebugFormat("{0}: delink {1}/{2}", LogHeader, _avName, _localID); + if (_parentPrim != null) + { + _parentPrim.RemoveChildFromLinkset(this); + } + return; + } + + public void AddChildToLinkset(BSPrim pchild) + { + BSPrim child = pchild; + _scene.TaintedObject(delegate() + { + if (!_childrenPrims.Contains(child)) + { + _childrenPrims.Add(child); + child.ParentPrim = this; // the child has gained a parent + RecreateGeomAndObject(); // rebuild my shape with the new child added + } + }); + return; + } + + public void RemoveChildFromLinkset(BSPrim pchild) + { + BSPrim child = pchild; + _scene.TaintedObject(delegate() + { + if (_childrenPrims.Contains(child)) + { + _childrenPrims.Remove(child); + child.ParentPrim = null; // the child has lost its parent + RecreateGeomAndObject(); // rebuild my shape with the child removed + } + else + { + m_log.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset"); + } + }); + return; + } + + public BSPrim ParentPrim + { + set { _parentPrim = value; } + } + + public ulong HullKey + { + get { return _hullKey; } + } + + // return true if we are the root of a linkset (there are children to manage) + public bool IsRootOfLinkset + { + get { return (_parentPrim == null && _childrenPrims.Count != 0); } + } + + public override void LockAngularMotion(OMV.Vector3 axis) { return; } + + public override OMV.Vector3 Position { + get { + // don't do the following GetObjectPosition because this function is called a zillion times + // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); + return _position; + } + set { + _position = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); + // m_log.DebugFormat("{0}: setPosition: id={1}, position={2}", LogHeader, _localID, _position); + }); + } + } + public override float Mass { + get { return _mass; } + } + public override OMV.Vector3 Force { + get { return _force; } + set { + _force = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); + }); + } + } + + public override int VehicleType { + get { + return (int)_vehicle.Type; // if we are a vehicle, return that type + } + set { + Vehicle type = (Vehicle)value; + _vehicle.ProcessTypeChange(type); + _scene.TaintedObject(delegate() + { + if (type == Vehicle.TYPE_NONE) + { + _scene.RemoveVehiclePrim(this); + } + else + { + // make it so the scene will call us each tick to do vehicle things + _scene.AddVehiclePrim(this); + } + return; + }); + } + } + public override void VehicleFloatParam(int param, float value) + { + _vehicle.ProcessFloatVehicleParam((Vehicle)param, value); + } + public override void VehicleVectorParam(int param, OMV.Vector3 value) + { + _vehicle.ProcessVectorVehicleParam((Vehicle)param, value); + } + public override void VehicleRotationParam(int param, OMV.Quaternion rotation) + { + _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); + } + public override void VehicleFlags(int param, bool remove) + { + _vehicle.ProcessVehicleFlags(param, remove); + } + // Called each simulation step to advance vehicle characteristics + public void StepVehicle(float timeStep) + { + _vehicle.Step(timeStep, _scene); + } + + // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more + public override void SetVolumeDetect(int param) { + bool newValue = (param != 0); + if (_isVolumeDetect != newValue) + { + _isVolumeDetect = newValue; + _scene.TaintedObject(delegate() + { + SetObjectDynamic(); + }); + } + return; + } + + public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } } + public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } } + public override OMV.Vector3 Velocity { + get { return _velocity; } + set { _velocity = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity); + }); + } + } + public OMV.Vector3 AngularVelocity + { + get { return _angularVelocity; } + set + { + _angularVelocity = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _angularVelocity); + }); + } + } + public override OMV.Vector3 Torque { + get { return _torque; } + set { _torque = value; + } + } + public override float CollisionScore { + get { return _collisionScore; } + set { _collisionScore = value; + } + } + public override OMV.Vector3 Acceleration { + get { return _acceleration; } + } + public override OMV.Quaternion Orientation { + get { return _orientation; } + set { + _orientation = value; + _scene.TaintedObject(delegate() + { + // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); + BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); + // m_log.DebugFormat("{0}: set orientation: {1}", LogHeader, _orientation); + }); + } + } + public override int PhysicsActorType { + get { return _physicsActorType; } + set { _physicsActorType = value; + } + } + public override bool IsPhysical { + get { return _isPhysical; } + set { + _isPhysical = value; + _scene.TaintedObject(delegate() + { + SetObjectDynamic(); + }); + } + } + + // An object is static (does not move) if selected or not physical + private bool IsStatic + { + get { return _isSelected || !IsPhysical; } + } + + // An object is solid if it's not phantom and if it's not doing VolumeDetect + private bool IsSolid + { + get { return !IsPhantom && !_isVolumeDetect; } + } + + // make gravity work if the object is physical and not selected + // no locking here because only called when it is safe + private void SetObjectDynamic() + { + // non-physical things work best with a mass of zero + _mass = IsStatic ? 0f : CalculateMass(); + BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), _mass); + // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}, mass={4}", LogHeader, _localID, IsStatic, IsSolid, _mass); + } + + // prims don't fly + public override bool Flying { + get { return _flying; } + set { _flying = value; } + } + public override bool SetAlwaysRun { + get { return _setAlwaysRun; } + set { _setAlwaysRun = value; } + } + public override bool ThrottleUpdates { + get { return _throttleUpdates; } + set { _throttleUpdates = value; } + } + public override bool IsColliding { + get { return (_collidingStep == _scene.SimulationStep); } + set { _isColliding = value; } + } + public override bool CollidingGround { + get { return (_collidingGroundStep == _scene.SimulationStep); } + set { _collidingGround = value; } + } + public override bool CollidingObj { + get { return _collidingObj; } + set { _collidingObj = value; } + } + public bool IsPhantom { + get { + // SceneObjectPart removes phantom objects from the physics scene + // so, although we could implement touching and such, we never + // are invoked as a phantom object + return false; + } + } + public override bool FloatOnWater { + set { _floatOnWater = value; } + } + public override OMV.Vector3 RotationalVelocity { + get { return _rotationalVelocity; } + set { _rotationalVelocity = value; + // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); + } + } + public override bool Kinematic { + get { return _kinematic; } + set { _kinematic = value; + // m_log.DebugFormat("{0}: Kinematic={1}", LogHeader, _kinematic); + } + } + public override float Buoyancy { + get { return _buoyancy; } + set { _buoyancy = value; + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy); + }); + } + } + + // Used for MoveTo + public override OMV.Vector3 PIDTarget { + set { _PIDTarget = value; } + } + public override bool PIDActive { + set { _usePID = value; } + } + public override float PIDTau { + set { _PIDTau = value; } + } + + // Used for llSetHoverHeight and maybe vehicle height + // Hover Height will override MoveTo target's Z + public override bool PIDHoverActive { + set { _useHoverPID = value; } + } + public override float PIDHoverHeight { + set { _PIDHoverHeight = value; } + } + public override PIDHoverType PIDHoverType { + set { _PIDHoverType = value; } + } + public override float PIDHoverTau { + set { _PIDHoverTao = value; } + } + + // For RotLookAt + public override OMV.Quaternion APIDTarget { set { return; } } + public override bool APIDActive { set { return; } } + public override float APIDStrength { set { return; } } + public override float APIDDamping { set { return; } } + + public override void AddForce(OMV.Vector3 force, bool pushforce) { + if (force.IsFinite()) + { + _force.X += force.X; + _force.Y += force.Y; + _force.Z += force.Z; + } + else + { + m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader); + } + _scene.TaintedObject(delegate() + { + BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); + }); + } + + public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { + // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce); + } + public override void SetMomentum(OMV.Vector3 momentum) { + } + public override void SubscribeEvents(int ms) { + _subscribedEventsMs = ms; + _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen + } + public override void UnSubscribeEvents() { + _subscribedEventsMs = 0; + } + public override bool SubscribedEvents() { + return (_subscribedEventsMs > 0); + } + + #region Mass Calculation + + private float CalculateMass() + { + float volume = _size.X * _size.Y * _size.Z; // default + float tmp; + + float returnMass = 0; + float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; + float hollowVolume = hollowAmount * hollowAmount; + + switch (_pbs.ProfileShape) + { + case ProfileShape.Square: + // default box + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + + hollowVolume *= 0.78539816339f; + break; + + case HollowShape.Triangle: + + hollowVolume *= (0.5f * .5f); + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + //a tube + + volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); + tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); + volume -= volume*tmp*tmp; + + if (hollowAmount > 0.0) + { + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Square: + case HollowShape.Same: + break; + + case HollowShape.Circle: + hollowVolume *= 0.78539816339f;; + break; + + case HollowShape.Triangle: + hollowVolume *= 0.5f * 0.5f; + break; + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + break; + + case ProfileShape.Circle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.78539816339f; // elipse base + + if (hollowAmount > 0.0) + { + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Circle: + break; + + case HollowShape.Square: + hollowVolume *= 0.5f * 2.5984480504799f; + break; + + case HollowShape.Triangle: + hollowVolume *= .5f * 1.27323954473516f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + case ProfileShape.HalfCircle: + if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.52359877559829887307710723054658f; + } + break; + + case ProfileShape.EquilateralTriangle: + + if (_pbs.PathCurve == (byte)Extrusion.Straight) + { + volume *= 0.32475953f; + + if (hollowAmount > 0.0) + { + + // calculate the hollow volume by it's shape compared to the prim shape + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + // Hollow shape is a perfect cyllinder in respect to the cube's scale + // Cyllinder hollow volume calculation + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + else if (_pbs.PathCurve == (byte)Extrusion.Curve1) + { + volume *= 0.32475953f; + volume *= 0.01f * (float)(200 - _pbs.PathScaleX); + tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); + volume *= (1.0f - tmp * tmp); + + if (hollowAmount > 0.0) + { + + hollowVolume *= hollowAmount; + + switch (_pbs.HollowShape) + { + case HollowShape.Same: + case HollowShape.Triangle: + hollowVolume *= .25f; + break; + + case HollowShape.Square: + hollowVolume *= 0.499849f * 3.07920140172638f; + break; + + case HollowShape.Circle: + + hollowVolume *= 0.1963495f * 3.07920140172638f; + break; + + default: + hollowVolume = 0; + break; + } + volume *= (1.0f - hollowVolume); + } + } + break; + + default: + break; + } + + + + float taperX1; + float taperY1; + float taperX; + float taperY; + float pathBegin; + float pathEnd; + float profileBegin; + float profileEnd; + + if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) + { + taperX1 = _pbs.PathScaleX * 0.01f; + if (taperX1 > 1.0f) + taperX1 = 2.0f - taperX1; + taperX = 1.0f - taperX1; + + taperY1 = _pbs.PathScaleY * 0.01f; + if (taperY1 > 1.0f) + taperY1 = 2.0f - taperY1; + taperY = 1.0f - taperY1; + } + else + { + taperX = _pbs.PathTaperX * 0.01f; + if (taperX < 0.0f) + taperX = -taperX; + taperX1 = 1.0f - taperX; + + taperY = _pbs.PathTaperY * 0.01f; + if (taperY < 0.0f) + taperY = -taperY; + taperY1 = 1.0f - taperY; + + } + + + volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); + + pathBegin = (float)_pbs.PathBegin * 2.0e-5f; + pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; + volume *= (pathEnd - pathBegin); + + // this is crude aproximation + profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; + profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; + volume *= (profileEnd - profileBegin); + + returnMass = _density * volume; + + if (returnMass <= 0) + returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. + + if (IsRootOfLinkset) + { + foreach (BSPrim prim in _childrenPrims) + { + returnMass += prim.CalculateMass(); + } + } + + if (returnMass > _scene.maximumMassObject) + returnMass = _scene.maximumMassObject; + return returnMass; + }// end CalculateMass + #endregion Mass Calculation + + // Create the geometry information in Bullet for later use + // No locking here because this is done when we know physics is not simulating + private void CreateGeom() + { + if (_mesh == null) + { + // the mesher thought this was too simple to mesh. Use a native Bullet collision shape. + if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) + { + if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) + { + // m_log.DebugFormat("{0}: CreateGeom: mesh null. Defaulting to sphere of size {1}", LogHeader, _size); + _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE; + // Bullet native objects are scaled by the Bullet engine so pass the size in + _scale = _size; + } + } + else + { + // m_log.DebugFormat("{0}: CreateGeom: mesh null. Defaulting to box of size {1}", LogHeader, _size); + _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX; + _scale = _size; + } + } + else + { + if (_hullKey != 0) + { + // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey); + BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey); + _hullKey = 0; + _hulls.Clear(); + } + + int[] indices = _mesh.getIndexListAsInt(); + List vertices = _mesh.getVertexList(); + + //format conversion from IMesh format to DecompDesc format + List convIndices = new List(); + List convVertices = new List(); + for (int ii = 0; ii < indices.GetLength(0); ii++) + { + convIndices.Add(indices[ii]); + } + foreach (OMV.Vector3 vv in vertices) + { + convVertices.Add(new float3(vv.X, vv.Y, vv.Z)); + } + + // setup and do convex hull conversion + _hulls = new List(); + DecompDesc dcomp = new DecompDesc(); + dcomp.mIndices = convIndices; + dcomp.mVertices = convVertices; + ConvexBuilder convexBuilder = new ConvexBuilder(HullReturn); + // create the hull into the _hulls variable + convexBuilder.process(dcomp); + + // Convert the vertices and indices for passing to unmanaged + // The hull information is passed as a large floating point array. + // The format is: + // convHulls[0] = number of hulls + // convHulls[1] = number of vertices in first hull + // convHulls[2] = hull centroid X coordinate + // convHulls[3] = hull centroid Y coordinate + // convHulls[4] = hull centroid Z coordinate + // convHulls[5] = first hull vertex X + // convHulls[6] = first hull vertex Y + // convHulls[7] = first hull vertex Z + // convHulls[8] = second hull vertex X + // ... + // convHulls[n] = number of vertices in second hull + // convHulls[n+1] = second hull centroid X coordinate + // ... + // + // TODO: is is very inefficient. Someday change the convex hull generator to return + // data structures that do not need to be converted in order to pass to Bullet. + // And maybe put the values directly into pinned memory rather than marshaling. + int hullCount = _hulls.Count; + int totalVertices = 1; // include one for the count of the hulls + foreach (ConvexResult cr in _hulls) + { + totalVertices += 4; // add four for the vertex count and centroid + totalVertices += cr.HullIndices.Count * 3; // we pass just triangles + } + float[] convHulls = new float[totalVertices]; + + convHulls[0] = (float)hullCount; + int jj = 1; + foreach (ConvexResult cr in _hulls) + { + // copy vertices for index access + float3[] verts = new float3[cr.HullVertices.Count]; + int kk = 0; + foreach (float3 ff in cr.HullVertices) + { + verts[kk++] = ff; + } + + // add to the array one hull's worth of data + convHulls[jj++] = cr.HullIndices.Count; + convHulls[jj++] = 0f; // centroid x,y,z + convHulls[jj++] = 0f; + convHulls[jj++] = 0f; + foreach (int ind in cr.HullIndices) + { + convHulls[jj++] = verts[ind].x; + convHulls[jj++] = verts[ind].y; + convHulls[jj++] = verts[ind].z; + } + } + + // create the hull definition in Bullet + _hullKey = (ulong)_pbs.GetHashCode(); + // m_log.DebugFormat("{0}: CreateGeom: calling CreateHull. lid= {1}, key={2}, hulls={3}", LogHeader, _localID, _hullKey, hullCount); + BulletSimAPI.CreateHull(_scene.WorldID, _hullKey, hullCount, convHulls); + _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL; + // meshes are already scaled by the meshmerizer + _scale = new OMV.Vector3(1f, 1f, 1f); + } + return; + } + + private void HullReturn(ConvexResult result) + { + _hulls.Add(result); + return; + } + + // Create an object in Bullet + // No locking here because this is done when the physics engine is not simulating + private void CreateObject() + { + if (IsRootOfLinkset) + { + // Create a linkset around this object + /* + * NOTE: the original way of creating a linkset was to create a compound hull in the + * root which consisted of the hulls of all the children. This didn't work well because + * OpenSimulator needs updates and collisions for all the children and the physics + * engine didn't create events for the children when the root hull was moved. + * This code creates the compound hull. + // If I am the root prim of a linkset, replace my physical shape with all the + // pieces of the children. + // All of the children should have called CreateGeom so they have a hull + // in the physics engine already. Here we pull together all of those hulls + // into one shape. + int totalPrimsInLinkset = _childrenPrims.Count + 1; + // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, totalPrimsInLinkset); + ShapeData[] shapes = new ShapeData[totalPrimsInLinkset]; + FillShapeInfo(out shapes[0]); + int ii = 1; + foreach (BSPrim prim in _childrenPrims) + { + // m_log.DebugFormat("{0}: CreateLinkset: adding prim {1}", LogHeader, prim.LocalID); + prim.FillShapeInfo(out shapes[ii]); + ii++; + } + BulletSimAPI.CreateLinkset(_scene.WorldID, totalPrimsInLinkset, shapes); + */ + // Create the linkset by putting constraints between the objects of the set so they cannot move + // relative to each other. + // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1); + + // remove any constraints that might be in place + foreach (BSPrim prim in _childrenPrims) + { + BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID); + } + // create constraints between the root prim and each of the children + foreach (BSPrim prim in _childrenPrims) + { + // this is a constraint that allows no freedom of movement between the two objects + // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 + BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, OMV.Vector3.Zero, OMV.Vector3.Zero, + OMV.Vector3.Zero, OMV.Vector3.Zero, OMV.Vector3.Zero, OMV.Vector3.Zero); + } + } + else + { + // simple object + // m_log.DebugFormat("{0}: CreateObject. ID={1}", LogHeader, LocalID); + ShapeData shape; + FillShapeInfo(out shape); + BulletSimAPI.CreateObject(_scene.WorldID, shape); + } + } + + // Copy prim's info into the BulletSim shape description structure + public void FillShapeInfo(out ShapeData shape) + { + shape.ID = _localID; + shape.Type = _shapeType; + shape.Position = _position; + shape.Rotation = _orientation; + shape.Velocity = _velocity; + shape.Scale = _scale; + shape.Mass = _isPhysical ? _mass : 0f; + shape.Buoyancy = _buoyancy; + shape.MeshKey = _hullKey; + shape.Collidable = (!IsPhantom) ? ShapeData.numericTrue : ShapeData.numericFalse; + shape.Friction = _friction; + shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue; + } + + // Rebuild the geometry and object. + // This is called when the shape changes so we need to recreate the mesh/hull. + // No locking here because this is done when the physics engine is not simulating + private void RecreateGeomAndObject() + { + if (_hullKey != 0) + { + // if a hull already exists, delete the old one + BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey); + _hullKey = 0; + } + // If this object is complex or we are the root of a linkset, build a mesh. + // The root of a linkset must be a mesh so we can create the linked compound object. + if (_scene.NeedsMeshing(_pbs) || IsRootOfLinkset ) + { + // m_log.DebugFormat("{0}: RecreateGeomAndObject: creating mesh", LogHeader); + _mesh = _scene.mesher.CreateMesh(_avName, _pbs, _size, _scene.meshLOD, _isPhysical); + } + else + { + // it's a BulletSim native shape. + _mesh = null; + } + CreateGeom(); // create the geometry for this prim + CreateObject(); + return; + } + + // The physics engine says that properties have updated. Update same and inform + // the world that things have changed. + // TODO: do we really need to check for changed? Maybe just copy values and call RequestPhysicsterseUpdate() + private int UpPropPosition = 1 << 0; + private int UpPropRotation = 1 << 1; + private int UpPropVelocity = 1 << 2; + private int UpPropAcceleration = 1 << 3; + private int UpPropAngularVel = 1 << 4; + + public void UpdateProperties(EntityProperties entprop) + { + int changed = 0; + // assign to the local variables so the normal set action does not happen + if (_position != entprop.Position) + { + _position = entprop.Position; + // m_log.DebugFormat("{0}: UpdateProperties: position = {1}", LogHeader, _position); + changed |= UpPropPosition; + } + if (_orientation != entprop.Rotation) + { + _orientation = entprop.Rotation; + // m_log.DebugFormat("{0}: UpdateProperties: rotation = {1}", LogHeader, _orientation); + changed |= UpPropRotation; + } + if (_velocity != entprop.Velocity) + { + _velocity = entprop.Velocity; + // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity); + changed |= UpPropVelocity; + } + if (_acceleration != entprop.Acceleration) + { + _acceleration = entprop.Acceleration; + // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration); + changed |= UpPropAcceleration; + } + if (_rotationalVelocity != entprop.AngularVelocity) + { + _rotationalVelocity = entprop.AngularVelocity; + // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity); + changed |= UpPropAngularVel; + } + if (changed != 0) + { + // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation); + base.RequestPhysicsterseUpdate(); + } + } + + // I've collided with something + public void Collide(uint collidingWith, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) + { + // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith); + // The following makes IsColliding() and IsCollidingGround() work + _collidingStep = _scene.SimulationStep; + if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID) + { + _collidingGroundStep = _scene.SimulationStep; + } + + if (_subscribedEventsMs == 0) return; // nothing in the object is waiting for collision events + // throttle the collisions to the number of milliseconds specified in the subscription + int nowTime = Util.EnvironmentTickCount(); + if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return; + _lastCollisionTime = nowTime; + + // create the event for the collision + Dictionary contactPoints = new Dictionary(); + contactPoints.Add(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); + CollisionEventUpdate args = new CollisionEventUpdate(LocalID, (int)type, 1, contactPoints); + base.SendCollisionUpdate(args); + } +} +} diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs new file mode 100644 index 0000000000..beffd21f40 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs @@ -0,0 +1,553 @@ +/* + * 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 copyrightD + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text; +using System.Threading; +using Nini.Config; +using log4net; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using OpenMetaverse; +using OpenSim.Region.Framework; + +// TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim) +// Fix folding up feet +// Fix terrain. Only flat terrain works. Terrain with shape is oriented wrong? Origined wrong? +// Parameterize BulletSim. Pass a structure of parameters to the C++ code. Capsule size, friction, ... +// Shift drag duplication of objects does not work +// Adjust character capsule size when height is adjusted (ScenePresence.SetHeight) +// Test sculpties +// Compute physics FPS reasonably +// Based on material, set density and friction +// More efficient memory usage in passing hull information from BSPrim to BulletSim +// Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly? +// In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground) +// At the moment, physical and phantom causes object to drop through the terrain +// Should prim.link() and prim.delink() membership checking happen at taint time? +// Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once +// Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect +// Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions) +// Implement LockAngularMotion +// Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation) +// Built Galton board (lots of MoveTo's) and some slats were not positioned correctly (mistakes scattered) +// No mistakes with ODE. Shape creation race condition? +// Does NeedsMeshing() really need to exclude all the different shapes? +// +namespace OpenSim.Region.Physics.BulletSPlugin +{ +public class BSScene : PhysicsScene +{ + private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); + private static readonly string LogHeader = "[BULLETS SCENE]"; + + private Dictionary m_avatars = new Dictionary(); + private Dictionary m_prims = new Dictionary(); + private List m_vehicles = new List(); + private float[] m_heightMap; + private float m_waterLevel; + private uint m_worldID; + public uint WorldID { get { return m_worldID; } } + + public IMesher mesher; + public int meshLOD = 32; + + private int m_maxSubSteps = 10; + private float m_fixedTimeStep = 1f / 60f; + private long m_simulationStep = 0; + public long SimulationStep { get { return m_simulationStep; } } + + private bool _meshSculptedPrim = true; // cause scuplted prims to get meshed + private bool _forceSimplePrimMeshing = false; // if a cube or sphere, let Bullet do internal shapes + public float maximumMassObject = 10000.01f; + + public const uint TERRAIN_ID = 0; + public const uint GROUNDPLANE_ID = 1; + + public float DefaultFriction = 0.70f; + public float DefaultDensity = 10.000006836f; // Aluminum g/cm3; TODO: compute based on object material + public Vector3 DefaultGravity = new Vector3(0, 0, -9.80665f); + + public delegate void TaintCallback(); + private List _taintedObjects; + private Object _taintLock = new Object(); + + private BulletSimAPI.DebugLogCallback debugLogCallbackHandle; + + public BSScene(string identifier) + { + } + + public override void Initialise(IMesher meshmerizer, IConfigSource config) + { + if (config != null) + { + IConfig pConfig = config.Configs["BulletSim"]; + if (pConfig != null) + { + DefaultFriction = pConfig.GetFloat("Friction", DefaultFriction); + DefaultDensity = pConfig.GetFloat("Density", DefaultDensity); + // TODO: a lot more parameters that are passed to BulletSim + } + } + // if Debug, enable logging from the unmanaged code + if (m_log.IsDebugEnabled) + { + m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader); + debugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger); + BulletSimAPI.SetDebugLogCallback(debugLogCallbackHandle); + } + + _meshSculptedPrim = true; // mesh sculpted prims + _forceSimplePrimMeshing = false; // use complex meshing if called for + + _taintedObjects = new List(); + + mesher = meshmerizer; + // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader); + m_worldID = BulletSimAPI.Initialize(new Vector3(Constants.RegionSize, Constants.RegionSize, 4096f)); + } + + // Called directly from unmanaged code so don't do much + private void BulletLogger(string msg) + { + m_log.Debug("[BULLETS UNMANAGED]:" + msg); + } + + public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) + { + m_log.ErrorFormat("{0}: CALL TO AddAvatar in BSScene. NOT IMPLEMENTED", LogHeader); + return null; + } + + public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, bool isFlying) + { + // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName); + BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying); + lock (m_avatars) m_avatars.Add(localID, actor); + return actor; + } + + public override void RemoveAvatar(PhysicsActor actor) + { + // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader); + if (actor is BSCharacter) + { + ((BSCharacter)actor).Destroy(); + } + try + { + lock (m_avatars) m_avatars.Remove(actor.LocalID); + } + catch (Exception e) + { + m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e); + } + } + + public override void RemovePrim(PhysicsActor prim) + { + // m_log.DebugFormat("{0}: RemovePrim", LogHeader); + if (prim is BSPrim) + { + ((BSPrim)prim).Destroy(); + } + try + { + lock (m_prims) m_prims.Remove(prim.LocalID); + } + catch (Exception e) + { + m_log.WarnFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e); + } + } + + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation) // deprecated + { + return null; + } + public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) + { + m_log.ErrorFormat("{0}: CALL TO AddPrimShape in BSScene. NOT IMPLEMENTED", LogHeader); + return null; + } + + public override PhysicsActor AddPrimShape(uint localID, string primName, PrimitiveBaseShape pbs, Vector3 position, + Vector3 size, Quaternion rotation, bool isPhysical) + { + // m_log.DebugFormat("{0}: AddPrimShape2: {1}", LogHeader, primName); + IMesh mesh = null; + if (NeedsMeshing(pbs)) + { + // if the prim is complex, create the mesh for it. + // If simple (box or sphere) leave 'mesh' null and physics will do a native shape. + mesh = mesher.CreateMesh(primName, pbs, size, this.meshLOD, isPhysical); + } + BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, mesh, pbs, isPhysical); + lock (m_prims) m_prims.Add(localID, prim); + return prim; + } + + // This is a call from the simulator saying that some physical property has been updated. + // The BulletS driver senses the changing of relevant properties so this taint + // information call is not needed. + public override void AddPhysicsActorTaint(PhysicsActor prim) { } + + // Simulate one timestep + public override float Simulate(float timeStep) + { + int updatedEntityCount; + IntPtr updatedEntitiesPtr; + IntPtr[] updatedEntities; + int collidersCount; + IntPtr collidersPtr; + int[] colliders; // should be uint but Marshal.Copy does not have that overload + + // update the prim states while we know the physics engine is not busy + ProcessTaints(); + + // Some of the prims operate with special vehicle properties + ProcessVehicles(timeStep); + ProcessTaints(); // the vehicles might have added taints + + // step the physical world one interval + m_simulationStep++; + int numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep, + out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr); + + // if there were collisions, they show up here + if (collidersCount > 0) + { + colliders = new int[collidersCount]; + Marshal.Copy(collidersPtr, colliders, 0, collidersCount); + for (int ii = 0; ii < collidersCount; ii+=2) + { + uint cA = (uint)colliders[ii]; + uint cB = (uint)colliders[ii+1]; + SendCollision(cA, cB); + SendCollision(cB, cA); + } + } + + // if any of the objects had updated properties, they are returned in the updatedEntity structure + // TODO: figure out how to pass all of the EntityProperties structures in one marshal call. + if (updatedEntityCount > 0) + { + updatedEntities = new IntPtr[updatedEntityCount]; + // fetch all the pointers to all the EntityProperties structures for these updates + Marshal.Copy(updatedEntitiesPtr, updatedEntities, 0, updatedEntityCount); + for (int ii = 0; ii < updatedEntityCount; ii++) + { + IntPtr updatePointer = updatedEntities[ii]; + EntityProperties entprop = (EntityProperties)Marshal.PtrToStructure(updatePointer, typeof(EntityProperties)); + // m_log.DebugFormat("{0}: entprop: id={1}, pos={2}", LogHeader, entprop.ID, entprop.Position); + BSCharacter actor; + if (m_avatars.TryGetValue(entprop.ID, out actor)) + { + actor.UpdateProperties(entprop); + continue; + } + BSPrim prim; + if (m_prims.TryGetValue(entprop.ID, out prim)) + { + prim.UpdateProperties(entprop); + } + } + } + + // fps calculation wrong. This calculation returns about 1 in normal operation. + return timeStep / (numSubSteps * m_fixedTimeStep) * 1000f; + } + + // Something has collided + private void SendCollision(uint localID, uint collidingWith) + { + if (localID == TERRAIN_ID || localID == GROUNDPLANE_ID) + { + // we never send collisions to the terrain + return; + } + + ActorTypes type = ActorTypes.Prim; + if (collidingWith == TERRAIN_ID || collidingWith == GROUNDPLANE_ID) + type = ActorTypes.Ground; + else if (m_avatars.ContainsKey(collidingWith)) + type = ActorTypes.Agent; + + BSPrim prim; + if (m_prims.TryGetValue(localID, out prim)) { + prim.Collide(collidingWith, type, Vector3.Zero, Vector3.UnitZ, 0.01f); + return; + } + BSCharacter actor; + if (m_avatars.TryGetValue(localID, out actor)) { + actor.Collide(collidingWith, type, Vector3.Zero, Vector3.UnitZ, 0.01f); + return; + } + return; + } + + public override void GetResults() { } + + public override void SetTerrain(float[] heightMap) { + m_log.DebugFormat("{0}: SetTerrain", LogHeader); + m_heightMap = heightMap; + this.TaintedObject(delegate() + { + BulletSimAPI.SetHeightmap(m_worldID, m_heightMap); + }); + } + + public float GetTerrainHeightAtXY(float tX, float tY) + { + return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)]; + } + + public override void SetWaterLevel(float baseheight) + { + m_waterLevel = baseheight; + } + public float GetWaterLevel() + { + return m_waterLevel; + } + + public override void DeleteTerrain() + { + m_log.DebugFormat("{0}: DeleteTerrain()", LogHeader); + } + + public override void Dispose() + { + m_log.DebugFormat("{0}: Dispose()", LogHeader); + } + + public override Dictionary GetTopColliders() + { + return new Dictionary(); + } + + public override bool IsThreaded { get { return false; } } + + /// + /// Routine to figure out if we need to mesh this prim with our mesher + /// + /// + /// true if the prim needs meshing + 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... + + // int iPropertiesNotSupportedDefault = 0; + + if (pbs.SculptEntry && !_meshSculptedPrim) + { + // m_log.DebugFormat("{0}: NeedsMeshing: scultpy mesh", LogHeader); + return false; + } + + // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since Bullet + // can use an internal representation for the prim + if (!_forceSimplePrimMeshing) + { + // m_log.DebugFormat("{0}: NeedsMeshing: simple mesh: profshape={1}, curve={2}", LogHeader, pbs.ProfileShape, pbs.PathCurve); + 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) + { + return false; + } + } + } + + /* TODO: verify that the mesher will now do all these shapes + if (pbs.ProfileHollow != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) + iPropertiesNotSupportedDefault++; + + if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) + iPropertiesNotSupportedDefault++; + + if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) + iPropertiesNotSupportedDefault++; + + if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) + iPropertiesNotSupportedDefault++; + + // test for torus + if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) + { + if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) + { + iPropertiesNotSupportedDefault++; + } + } + else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) + { + if (pbs.PathCurve == (byte)Extrusion.Straight) + { + iPropertiesNotSupportedDefault++; + } + else if (pbs.PathCurve == (byte)Extrusion.Curve1) + { + iPropertiesNotSupportedDefault++; + } + } + if (iPropertiesNotSupportedDefault == 0) + { + return false; + } + */ + return true; + } + + // The calls to the PhysicsActors can't directly call into the physics engine + // because it might be busy. We we delay changes to a known time. + // We rely on C#'s closure to save and restore the context for the delegate. + public void TaintedObject(TaintCallback callback) + { + lock (_taintLock) + _taintedObjects.Add(callback); + return; + } + + // When someone tries to change a property on a BSPrim or BSCharacter, the object queues + // a callback into itself to do the actual property change. That callback is called + // here just before the physics engine is called to step the simulation. + public void ProcessTaints() + { + if (_taintedObjects.Count > 0) // save allocating new list if there is nothing to process + { + // swizzle a new list into the list location so we can process what's there + List oldList; + lock (_taintLock) + { + oldList = _taintedObjects; + _taintedObjects = new List(); + } + + foreach (TaintCallback callback in oldList) + { + try + { + callback(); + } + catch (Exception e) + { + m_log.ErrorFormat("{0}: ProcessTaints: Exception: {1}", LogHeader, e); + } + } + oldList.Clear(); + } + } + + #region Vehicles + // Make so the scene will call this prim for vehicle actions each tick. + // Safe to call if prim is already in the vehicle list. + public void AddVehiclePrim(BSPrim vehicle) + { + lock (m_vehicles) + { + if (!m_vehicles.Contains(vehicle)) + { + m_vehicles.Add(vehicle); + } + } + } + + // Remove a prim from our list of vehicles. + // Safe to call if the prim is not in the vehicle list. + public void RemoveVehiclePrim(BSPrim vehicle) + { + lock (m_vehicles) + { + if (m_vehicles.Contains(vehicle)) + { + m_vehicles.Remove(vehicle); + } + } + } + + // Some prims have extra vehicle actions + // no locking because only called when physics engine is not busy + private void ProcessVehicles(float timeStep) + { + foreach (BSPrim prim in m_vehicles) + { + prim.StepVehicle(timeStep); + } + } + #endregion Vehicles +} +} diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs new file mode 100644 index 0000000000..b2bc0d77a0 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs @@ -0,0 +1,186 @@ +/* + * 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 copyrightD + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the OpenSimulator Project nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +using System; +using System.Runtime.InteropServices; +using System.Security; +using System.Text; +using OpenMetaverse; + +namespace OpenSim.Region.Physics.BulletSPlugin { + +public struct ConvexHull +{ + Vector3 Offset; + int VertexCount; + Vector3[] Vertices; +} +public struct ShapeData +{ + public enum PhysicsShapeType + { + SHAPE_AVATAR = 0, + SHAPE_BOX = 1, + SHAPE_CONE = 2, + SHAPE_CYLINDER = 3, + SHAPE_SPHERE = 4, + SHAPE_HULL = 5 + }; + public const int numericTrue = 1; + public const int numericFalse = 0; + public uint ID; + public PhysicsShapeType Type; + public Vector3 Position; + public Quaternion Rotation; + public Vector3 Velocity; + public Vector3 Scale; + public float Mass; + public float Buoyancy; + public System.UInt64 MeshKey; + public int Collidable; + public float Friction; + public int Static; // true if a static object. Otherwise gravity, etc. + // note that bools are passed as ints since bool size changes by language +} +public struct SweepHit +{ + public uint ID; + public float Fraction; + public Vector3 Normal; + public Vector3 Point; +} +public struct RaycastHit +{ + public uint ID; + public float Fraction; + public Vector3 Normal; +} + +public struct EntityProperties +{ + public uint ID; + public Vector3 Position; + public Quaternion Rotation; + public Vector3 Velocity; + public Vector3 Acceleration; + public Vector3 AngularVelocity; +} + +static class BulletSimAPI { + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern uint Initialize(Vector3 maxPosition); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void SetHeightmap(uint worldID, [MarshalAs(UnmanagedType.LPArray)] float[] heightMap); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void Shutdown(uint worldID); + + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern int PhysicsStep(uint worldID, float timeStep, int maxSubSteps, float fixedTimeStep, + out int updatedEntityCount, + out IntPtr updatedEntitiesPtr, + out int collidersCount, + out IntPtr collidersPtr); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool CreateHull(uint worldID, System.UInt64 meshKey, int hullCount, + [MarshalAs(UnmanagedType.LPArray)] float[] hulls + ); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool DestroyHull(uint worldID, System.UInt64 meshKey); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool CreateObject(uint worldID, ShapeData shapeData); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void AddConstraint(uint worldID, uint id1, uint id2, + Vector3 frame1, Vector3 frame2, Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern Vector3 GetObjectPosition(uint WorldID, uint id); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectVelocity(uint worldID, uint id, Vector3 velocity); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectAngularVelocity(uint worldID, uint id, Vector3 angularVelocity); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectForce(uint worldID, uint id, Vector3 force); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectScaleMass(uint worldID, uint id, Vector3 scale, float mass, bool isDynamic); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectCollidable(uint worldID, uint id, bool phantom); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectDynamic(uint worldID, uint id, bool isDynamic, float mass); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectGhost(uint worldID, uint id, bool ghostly); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectProperties(uint worldID, uint id, bool isStatic, bool isSolid, bool genCollisions, float mass); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool SetObjectBuoyancy(uint worldID, uint id, float buoyancy); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool HasObject(uint worldID, uint id); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern bool DestroyObject(uint worldID, uint id); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern SweepHit ConvexSweepTest(uint worldID, uint id, Vector3 to, float extraMargin); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern RaycastHit RayTest(uint worldID, uint id, Vector3 from, Vector3 to); + +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern Vector3 RecoverFromPenetration(uint worldID, uint id); + +// Log a debug message +[UnmanagedFunctionPointer(CallingConvention.Cdecl)] +public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg); +[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] +public static extern void SetDebugLogCallback(DebugLogCallback callback); +} +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs new file mode 100644 index 0000000000..4d84c44704 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs @@ -0,0 +1,341 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class Wpoint + { + public float3 mPoint; + public float mWeight; + + public Wpoint(float3 p, float w) + { + mPoint = p; + mWeight = w; + } + } + + public class CTri + { + private const int WSCALE = 4; + + public float3 mP1; + public float3 mP2; + public float3 mP3; + public float3 mNear1; + public float3 mNear2; + public float3 mNear3; + public float3 mNormal; + public float mPlaneD; + public float mConcavity; + public float mC1; + public float mC2; + public float mC3; + public int mI1; + public int mI2; + public int mI3; + public int mProcessed; // already been added... + + public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3) + { + mProcessed = 0; + mI1 = i1; + mI2 = i2; + mI3 = i3; + + mP1 = new float3(p1); + mP2 = new float3(p2); + mP3 = new float3(p3); + + mNear1 = new float3(); + mNear2 = new float3(); + mNear3 = new float3(); + + mNormal = new float3(); + mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3); + } + + public float Facing(CTri t) + { + return float3.dot(mNormal, t.mNormal); + } + + public bool clip(float3 start, ref float3 end) + { + float3 sect = new float3(); + bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect); + + if (hit) + end = sect; + return hit; + } + + public bool Concave(float3 p, ref float distance, ref float3 n) + { + n.NearestPointInTriangle(p, mP1, mP2, mP3); + distance = p.Distance(n); + return true; + } + + public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount) + { + indices[tcount * 3 + 0] = i1; + indices[tcount * 3 + 1] = i2; + indices[tcount * 3 + 2] = i3; + tcount++; + } + + public float getVolume() + { + int[] indices = new int[8 * 3]; + + int tcount = 0; + + addTri(indices, 0, 1, 2, ref tcount); + addTri(indices, 3, 4, 5, ref tcount); + + addTri(indices, 0, 3, 4, ref tcount); + addTri(indices, 0, 4, 1, ref tcount); + + addTri(indices, 1, 4, 5, ref tcount); + addTri(indices, 1, 5, 2, ref tcount); + + addTri(indices, 0, 3, 5, ref tcount); + addTri(indices, 0, 5, 2, ref tcount); + + List vertices = new List { mP1, mP2, mP3, mNear1, mNear2, mNear3 }; + List indexList = new List(indices); + + float v = Concavity.computeMeshVolume(vertices, indexList); + return v; + } + + public float raySect(float3 p, float3 dir, ref float3 sect) + { + float4 plane = new float4(); + + plane.x = mNormal.x; + plane.y = mNormal.y; + plane.z = mNormal.z; + plane.w = mPlaneD; + + float3 dest = p + dir * 100000f; + + intersect(p, dest, ref sect, plane); + + return sect.Distance(p); // return the intersection distance + } + + public float planeDistance(float3 p) + { + float4 plane = new float4(); + + plane.x = mNormal.x; + plane.y = mNormal.y; + plane.z = mNormal.z; + plane.w = mPlaneD; + + return DistToPt(p, plane); + } + + public bool samePlane(CTri t) + { + const float THRESH = 0.001f; + float dd = Math.Abs(t.mPlaneD - mPlaneD); + if (dd > THRESH) + return false; + dd = Math.Abs(t.mNormal.x - mNormal.x); + if (dd > THRESH) + return false; + dd = Math.Abs(t.mNormal.y - mNormal.y); + if (dd > THRESH) + return false; + dd = Math.Abs(t.mNormal.z - mNormal.z); + if (dd > THRESH) + return false; + return true; + } + + public bool hasIndex(int i) + { + if (i == mI1 || i == mI2 || i == mI3) + return true; + return false; + } + + public bool sharesEdge(CTri t) + { + bool ret = false; + uint count = 0; + + if (t.hasIndex(mI1)) + count++; + if (t.hasIndex(mI2)) + count++; + if (t.hasIndex(mI3)) + count++; + + if (count >= 2) + ret = true; + + return ret; + } + + public float area() + { + float a = mConcavity * mP1.Area(mP2, mP3); + return a; + } + + public void addWeighted(List list) + { + Wpoint p1 = new Wpoint(mP1, mC1); + Wpoint p2 = new Wpoint(mP2, mC2); + Wpoint p3 = new Wpoint(mP3, mC3); + + float3 d1 = mNear1 - mP1; + float3 d2 = mNear2 - mP2; + float3 d3 = mNear3 - mP3; + + d1 *= WSCALE; + d2 *= WSCALE; + d3 *= WSCALE; + + d1 = d1 + mP1; + d2 = d2 + mP2; + d3 = d3 + mP3; + + Wpoint p4 = new Wpoint(d1, mC1); + Wpoint p5 = new Wpoint(d2, mC2); + Wpoint p6 = new Wpoint(d3, mC3); + + list.Add(p1); + list.Add(p2); + list.Add(p3); + + list.Add(p4); + list.Add(p5); + list.Add(p6); + } + + private static float DistToPt(float3 p, float4 plane) + { + float x = p.x; + float y = p.y; + float z = p.z; + float d = x*plane.x + y*plane.y + z*plane.z + plane.w; + return d; + } + + private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane) + { + float dp1 = DistToPt(p1, plane); + + float3 dir = new float3(); + dir.x = p2[0] - p1[0]; + dir.y = p2[1] - p1[1]; + dir.z = p2[2] - p1[2]; + + float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; + float dot2 = dp1 - plane[3]; + + float t = -(plane[3] + dot2) / dot1; + + split.x = (dir[0] * t) + p1[0]; + split.y = (dir[1] * t) + p1[1]; + split.z = (dir[2] * t) + p1[2]; + } + + private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t) + { + t = 0f; + + float3 e1, e2, h, s, q; + float a, f, u, v; + + e1 = v1 - v0; + e2 = v2 - v0; + h = float3.cross(d, e2); + a = float3.dot(e1, h); + + if (a > -0.00001f && a < 0.00001f) + return false; + + f = 1f / a; + s = p - v0; + u = f * float3.dot(s, h); + + if (u < 0.0f || u > 1.0f) + return false; + + q = float3.cross(s, e1); + v = f * float3.dot(d, q); + if (v < 0.0f || u + v > 1.0f) + return false; + + // at this stage we can compute t to find out where + // the intersection point is on the line + t = f * float3.dot(e2, q); + if (t > 0f) // ray intersection + return true; + else // this means that there is a line intersection but not a ray intersection + return false; + } + + private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect) + { + float3 dir = rayEnd - rayStart; + + float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); + float r = 1.0f / d; + + dir *= r; + + float t; + bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t); + + if (ret) + { + if (t > d) + { + sect.x = rayStart.x + dir.x * t; + sect.y = rayStart.y + dir.y * t; + sect.z = rayStart.z + dir.z * t; + } + else + { + ret = false; + } + } + + return ret; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs new file mode 100644 index 0000000000..cc6383a9c6 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs @@ -0,0 +1,233 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Text; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public static class Concavity + { + // compute's how 'concave' this object is and returns the total volume of the + // convex hull as well as the volume of the 'concavity' which was found. + public static float computeConcavity(List vertices, List indices, ref float4 plane, ref float volume) + { + float cret = 0f; + volume = 1f; + + HullResult result = new HullResult(); + HullDesc desc = new HullDesc(); + + desc.MaxFaces = 256; + desc.MaxVertices = 256; + desc.SetHullFlag(HullFlag.QF_TRIANGLES); + desc.Vertices = vertices; + + HullError ret = HullUtils.CreateConvexHull(desc, ref result); + + if (ret == HullError.QE_OK) + { + volume = computeMeshVolume2(result.OutputVertices, result.Indices); + + // ok..now..for each triangle on the original mesh.. + // we extrude the points to the nearest point on the hull. + List tris = new List(); + + for (int i = 0; i < result.Indices.Count / 3; i++) + { + int i1 = result.Indices[i * 3 + 0]; + int i2 = result.Indices[i * 3 + 1]; + int i3 = result.Indices[i * 3 + 2]; + + float3 p1 = result.OutputVertices[i1]; + float3 p2 = result.OutputVertices[i2]; + float3 p3 = result.OutputVertices[i3]; + + CTri t = new CTri(p1, p2, p3, i1, i2, i3); + tris.Add(t); + } + + // we have not pre-computed the plane equation for each triangle in the convex hull.. + float totalVolume = 0; + + List ftris = new List(); // 'feature' triangles. + List input_mesh = new List(); + + for (int i = 0; i < indices.Count / 3; i++) + { + int i1 = indices[i * 3 + 0]; + int i2 = indices[i * 3 + 1]; + int i3 = indices[i * 3 + 2]; + + float3 p1 = vertices[i1]; + float3 p2 = vertices[i2]; + float3 p3 = vertices[i3]; + + CTri t = new CTri(p1, p2, p3, i1, i2, i3); + input_mesh.Add(t); + } + + for (int i = 0; i < indices.Count / 3; i++) + { + int i1 = indices[i * 3 + 0]; + int i2 = indices[i * 3 + 1]; + int i3 = indices[i * 3 + 2]; + + float3 p1 = vertices[i1]; + float3 p2 = vertices[i2]; + float3 p3 = vertices[i3]; + + CTri t = new CTri(p1, p2, p3, i1, i2, i3); + + featureMatch(t, tris, input_mesh); + + if (t.mConcavity > 0.05f) + { + float v = t.getVolume(); + totalVolume += v; + ftris.Add(t); + } + } + + SplitPlane.computeSplitPlane(vertices, indices, ref plane); + cret = totalVolume; + } + + return cret; + } + + public static bool featureMatch(CTri m, List tris, List input_mesh) + { + bool ret = false; + float neardot = 0.707f; + m.mConcavity = 0; + + for (int i = 0; i < tris.Count; i++) + { + CTri t = tris[i]; + + if (t.samePlane(m)) + { + ret = false; + break; + } + + float dot = float3.dot(t.mNormal, m.mNormal); + + if (dot > neardot) + { + float d1 = t.planeDistance(m.mP1); + float d2 = t.planeDistance(m.mP2); + float d3 = t.planeDistance(m.mP3); + + if (d1 > 0.001f || d2 > 0.001f || d3 > 0.001f) // can't be near coplaner! + { + neardot = dot; + + t.raySect(m.mP1, m.mNormal, ref m.mNear1); + t.raySect(m.mP2, m.mNormal, ref m.mNear2); + t.raySect(m.mP3, m.mNormal, ref m.mNear3); + + ret = true; + } + } + } + + if (ret) + { + m.mC1 = m.mP1.Distance(m.mNear1); + m.mC2 = m.mP2.Distance(m.mNear2); + m.mC3 = m.mP3.Distance(m.mNear3); + + m.mConcavity = m.mC1; + + if (m.mC2 > m.mConcavity) + m.mConcavity = m.mC2; + if (m.mC3 > m.mConcavity) + m.mConcavity = m.mC3; + } + + return ret; + } + + private static float det(float3 p1, float3 p2, float3 p3) + { + return p1.x * p2.y * p3.z + p2.x * p3.y * p1.z + p3.x * p1.y * p2.z - p1.x * p3.y * p2.z - p2.x * p1.y * p3.z - p3.x * p2.y * p1.z; + } + + public static float computeMeshVolume(List vertices, List indices) + { + float volume = 0f; + + for (int i = 0; i < indices.Count / 3; i++) + { + float3 p1 = vertices[indices[i * 3 + 0]]; + float3 p2 = vertices[indices[i * 3 + 1]]; + float3 p3 = vertices[indices[i * 3 + 2]]; + + volume += det(p1, p2, p3); // compute the volume of the tetrahedran relative to the origin. + } + + volume *= (1.0f / 6.0f); + if (volume < 0f) + return -volume; + return volume; + } + + public static float computeMeshVolume2(List vertices, List indices) + { + float volume = 0f; + + float3 p0 = vertices[0]; + for (int i = 0; i < indices.Count / 3; i++) + { + float3 p1 = vertices[indices[i * 3 + 0]]; + float3 p2 = vertices[indices[i * 3 + 1]]; + float3 p3 = vertices[indices[i * 3 + 2]]; + + volume += tetVolume(p0, p1, p2, p3); // compute the volume of the tetrahedron relative to the root vertice + } + + return volume * (1.0f / 6.0f); + } + + private static float tetVolume(float3 p0, float3 p1, float3 p2, float3 p3) + { + float3 a = p1 - p0; + float3 b = p2 - p0; + float3 c = p3 - p0; + + float3 cross = float3.cross(b, c); + float volume = float3.dot(a, cross); + + if (volume < 0f) + return -volume; + return volume; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs new file mode 100644 index 0000000000..dfaede1176 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs @@ -0,0 +1,411 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class DecompDesc + { + public List mVertices; + public List mIndices; + + // options + public uint mDepth; // depth to split, a maximum of 10, generally not over 7. + public float mCpercent; // the concavity threshold percentage. 0=20 is reasonable. + public float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable. + + // hull output limits. + public uint mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less. + public float mSkinWidth; // a skin width to apply to the output hulls. + + public ConvexDecompositionCallback mCallback; // the interface to receive back the results. + + public DecompDesc() + { + mDepth = 5; + mCpercent = 5; + mPpercent = 5; + mMaxVertices = 32; + } + } + + public class CHull + { + public float[] mMin = new float[3]; + public float[] mMax = new float[3]; + public float mVolume; + public float mDiagonal; + public ConvexResult mResult; + + public CHull(ConvexResult result) + { + mResult = new ConvexResult(result); + mVolume = Concavity.computeMeshVolume(result.HullVertices, result.HullIndices); + + mDiagonal = getBoundingRegion(result.HullVertices, mMin, mMax); + + float dx = mMax[0] - mMin[0]; + float dy = mMax[1] - mMin[1]; + float dz = mMax[2] - mMin[2]; + + dx *= 0.1f; // inflate 1/10th on each edge + dy *= 0.1f; // inflate 1/10th on each edge + dz *= 0.1f; // inflate 1/10th on each edge + + mMin[0] -= dx; + mMin[1] -= dy; + mMin[2] -= dz; + + mMax[0] += dx; + mMax[1] += dy; + mMax[2] += dz; + } + + public void Dispose() + { + mResult = null; + } + + public bool overlap(CHull h) + { + return overlapAABB(mMin, mMax, h.mMin, h.mMax); + } + + // returns the d1Giagonal distance + private static float getBoundingRegion(List points, float[] bmin, float[] bmax) + { + float3 first = points[0]; + + bmin[0] = first.x; + bmin[1] = first.y; + bmin[2] = first.z; + + bmax[0] = first.x; + bmax[1] = first.y; + bmax[2] = first.z; + + for (int i = 1; i < points.Count; i++) + { + float3 p = points[i]; + + if (p[0] < bmin[0]) bmin[0] = p[0]; + if (p[1] < bmin[1]) bmin[1] = p[1]; + if (p[2] < bmin[2]) bmin[2] = p[2]; + + if (p[0] > bmax[0]) bmax[0] = p[0]; + if (p[1] > bmax[1]) bmax[1] = p[1]; + if (p[2] > bmax[2]) bmax[2] = p[2]; + } + + float dx = bmax[0] - bmin[0]; + float dy = bmax[1] - bmin[1]; + float dz = bmax[2] - bmin[2]; + + return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); + } + + // return true if the two AABB's overlap. + private static bool overlapAABB(float[] bmin1, float[] bmax1, float[] bmin2, float[] bmax2) + { + if (bmax2[0] < bmin1[0]) return false; // if the maximum is less than our minimum on any axis + if (bmax2[1] < bmin1[1]) return false; + if (bmax2[2] < bmin1[2]) return false; + + if (bmin2[0] > bmax1[0]) return false; // if the minimum is greater than our maximum on any axis + if (bmin2[1] > bmax1[1]) return false; // if the minimum is greater than our maximum on any axis + if (bmin2[2] > bmax1[2]) return false; // if the minimum is greater than our maximum on any axis + + return true; // the extents overlap + } + } + + public class ConvexBuilder + { + public List mChulls = new List(); + private ConvexDecompositionCallback mCallback; + + private int MAXDEPTH = 8; + private float CONCAVE_PERCENT = 1f; + private float MERGE_PERCENT = 2f; + + public ConvexBuilder(ConvexDecompositionCallback callback) + { + mCallback = callback; + } + + public void Dispose() + { + int i; + for (i = 0; i < mChulls.Count; i++) + { + CHull cr = mChulls[i]; + cr.Dispose(); + } + } + + public bool isDuplicate(uint i1, uint i2, uint i3, uint ci1, uint ci2, uint ci3) + { + uint dcount = 0; + + Debug.Assert(i1 != i2 && i1 != i3 && i2 != i3); + Debug.Assert(ci1 != ci2 && ci1 != ci3 && ci2 != ci3); + + if (i1 == ci1 || i1 == ci2 || i1 == ci3) + dcount++; + if (i2 == ci1 || i2 == ci2 || i2 == ci3) + dcount++; + if (i3 == ci1 || i3 == ci2 || i3 == ci3) + dcount++; + + return dcount == 3; + } + + public void getMesh(ConvexResult cr, VertexPool vc, List indices) + { + List src = cr.HullIndices; + + for (int i = 0; i < src.Count / 3; i++) + { + int i1 = src[i * 3 + 0]; + int i2 = src[i * 3 + 1]; + int i3 = src[i * 3 + 2]; + + float3 p1 = cr.HullVertices[i1]; + float3 p2 = cr.HullVertices[i2]; + float3 p3 = cr.HullVertices[i3]; + + i1 = vc.getIndex(p1); + i2 = vc.getIndex(p2); + i3 = vc.getIndex(p3); + } + } + + public CHull canMerge(CHull a, CHull b) + { + if (!a.overlap(b)) // if their AABB's (with a little slop) don't overlap, then return. + return null; + + CHull ret = null; + + // ok..we are going to combine both meshes into a single mesh + // and then we are going to compute the concavity... + + VertexPool vc = new VertexPool(); + + List indices = new List(); + + getMesh(a.mResult, vc, indices); + getMesh(b.mResult, vc, indices); + + int vcount = vc.GetSize(); + List vertices = vc.GetVertices(); + int tcount = indices.Count / 3; + + //don't do anything if hull is empty + if (tcount == 0) + { + vc.Clear(); + return null; + } + + HullResult hresult = new HullResult(); + HullDesc desc = new HullDesc(); + + desc.SetHullFlag(HullFlag.QF_TRIANGLES); + desc.Vertices = vertices; + + HullError hret = HullUtils.CreateConvexHull(desc, ref hresult); + + if (hret == HullError.QE_OK) + { + float combineVolume = Concavity.computeMeshVolume(hresult.OutputVertices, hresult.Indices); + float sumVolume = a.mVolume + b.mVolume; + + float percent = (sumVolume * 100) / combineVolume; + if (percent >= (100.0f - MERGE_PERCENT)) + { + ConvexResult cr = new ConvexResult(hresult.OutputVertices, hresult.Indices); + ret = new CHull(cr); + } + } + + vc.Clear(); + return ret; + } + + public bool combineHulls() + { + bool combine = false; + + sortChulls(mChulls); // sort the convex hulls, largest volume to least... + + List output = new List(); // the output hulls... + + int i; + for (i = 0; i < mChulls.Count && !combine; ++i) + { + CHull cr = mChulls[i]; + + int j; + for (j = 0; j < mChulls.Count; j++) + { + CHull match = mChulls[j]; + + if (cr != match) // don't try to merge a hull with itself, that be stoopid + { + + CHull merge = canMerge(cr, match); // if we can merge these two.... + + if (merge != null) + { + output.Add(merge); + + ++i; + while (i != mChulls.Count) + { + CHull cr2 = mChulls[i]; + if (cr2 != match) + { + output.Add(cr2); + } + i++; + } + + cr.Dispose(); + match.Dispose(); + combine = true; + break; + } + } + } + + if (combine) + { + break; + } + else + { + output.Add(cr); + } + } + + if (combine) + { + mChulls.Clear(); + mChulls = output; + output.Clear(); + } + + return combine; + } + + public int process(DecompDesc desc) + { + int ret = 0; + + MAXDEPTH = (int)desc.mDepth; + CONCAVE_PERCENT = desc.mCpercent; + MERGE_PERCENT = desc.mPpercent; + + ConvexDecomposition.calcConvexDecomposition(desc.mVertices, desc.mIndices, ConvexDecompResult, 0f, 0, MAXDEPTH, CONCAVE_PERCENT, MERGE_PERCENT); + + while (combineHulls()) // keep combinging hulls until I can't combine any more... + ; + + int i; + for (i = 0; i < mChulls.Count; i++) + { + CHull cr = mChulls[i]; + + // before we hand it back to the application, we need to regenerate the hull based on the + // limits given by the user. + + ConvexResult c = cr.mResult; // the high resolution hull... + + HullResult result = new HullResult(); + HullDesc hdesc = new HullDesc(); + + hdesc.SetHullFlag(HullFlag.QF_TRIANGLES); + + hdesc.Vertices = c.HullVertices; + hdesc.MaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output + + if (desc.mSkinWidth != 0f) + { + hdesc.SkinWidth = desc.mSkinWidth; + hdesc.SetHullFlag(HullFlag.QF_SKIN_WIDTH); // do skin width computation. + } + + HullError ret2 = HullUtils.CreateConvexHull(hdesc, ref result); + + if (ret2 == HullError.QE_OK) + { + ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); + + r.mHullVolume = Concavity.computeMeshVolume(result.OutputVertices, result.Indices); // the volume of the hull. + + // compute the best fit OBB + //computeBestFitOBB(result.mNumOutputVertices, result.mOutputVertices, sizeof(float) * 3, r.mOBBSides, r.mOBBTransform); + + //r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] * r.mOBBSides[2]; // compute the OBB volume. + + //fm_getTranslation(r.mOBBTransform, r.mOBBCenter); // get the translation component of the 4x4 matrix. + + //fm_matrixToQuat(r.mOBBTransform, r.mOBBOrientation); // extract the orientation as a quaternion. + + //r.mSphereRadius = computeBoundingSphere(result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter); + //r.mSphereVolume = fm_sphereVolume(r.mSphereRadius); + + mCallback(r); + } + + result = null; + cr.Dispose(); + } + + ret = mChulls.Count; + + mChulls.Clear(); + + return ret; + } + + public void ConvexDecompResult(ConvexResult result) + { + CHull ch = new CHull(result); + mChulls.Add(ch); + } + + public void sortChulls(List hulls) + { + hulls.Sort(delegate(CHull a, CHull b) { return a.mVolume.CompareTo(b.mVolume); }); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs new file mode 100644 index 0000000000..2e2bb70c7a --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs @@ -0,0 +1,200 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public delegate void ConvexDecompositionCallback(ConvexResult result); + + public class FaceTri + { + public float3 P1; + public float3 P2; + public float3 P3; + + public FaceTri() { } + + public FaceTri(List vertices, int i1, int i2, int i3) + { + P1 = new float3(vertices[i1]); + P2 = new float3(vertices[i2]); + P3 = new float3(vertices[i3]); + } + } + + public static class ConvexDecomposition + { + private static void addTri(VertexPool vl, List list, float3 p1, float3 p2, float3 p3) + { + int i1 = vl.getIndex(p1); + int i2 = vl.getIndex(p2); + int i3 = vl.getIndex(p3); + + // do *not* process degenerate triangles! + if ( i1 != i2 && i1 != i3 && i2 != i3 ) + { + list.Add(i1); + list.Add(i2); + list.Add(i3); + } + } + + public static void calcConvexDecomposition(List vertices, List indices, ConvexDecompositionCallback callback, float masterVolume, int depth, + int maxDepth, float concavePercent, float mergePercent) + { + float4 plane = new float4(); + bool split = false; + + if (depth < maxDepth) + { + float volume = 0f; + float c = Concavity.computeConcavity(vertices, indices, ref plane, ref volume); + + if (depth == 0) + { + masterVolume = volume; + } + + float percent = (c * 100.0f) / masterVolume; + + if (percent > concavePercent) // if great than 5% of the total volume is concave, go ahead and keep splitting. + { + split = true; + } + } + + if (depth >= maxDepth || !split) + { + HullResult result = new HullResult(); + HullDesc desc = new HullDesc(); + + desc.SetHullFlag(HullFlag.QF_TRIANGLES); + + desc.Vertices = vertices; + + HullError ret = HullUtils.CreateConvexHull(desc, ref result); + + if (ret == HullError.QE_OK) + { + ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); + callback(r); + } + + return; + } + + List ifront = new List(); + List iback = new List(); + + VertexPool vfront = new VertexPool(); + VertexPool vback = new VertexPool(); + + // ok..now we are going to 'split' all of the input triangles against this plane! + for (int i = 0; i < indices.Count / 3; i++) + { + int i1 = indices[i * 3 + 0]; + int i2 = indices[i * 3 + 1]; + int i3 = indices[i * 3 + 2]; + + FaceTri t = new FaceTri(vertices, i1, i2, i3); + + float3[] front = new float3[4]; + float3[] back = new float3[4]; + + int fcount = 0; + int bcount = 0; + + PlaneTriResult result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); + + if (fcount > 4 || bcount > 4) + { + result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); + } + + switch (result) + { + case PlaneTriResult.PTR_FRONT: + Debug.Assert(fcount == 3); + addTri(vfront, ifront, front[0], front[1], front[2]); + break; + case PlaneTriResult.PTR_BACK: + Debug.Assert(bcount == 3); + addTri(vback, iback, back[0], back[1], back[2]); + break; + case PlaneTriResult.PTR_SPLIT: + Debug.Assert(fcount >= 3 && fcount <= 4); + Debug.Assert(bcount >= 3 && bcount <= 4); + + addTri(vfront, ifront, front[0], front[1], front[2]); + addTri(vback, iback, back[0], back[1], back[2]); + + if (fcount == 4) + { + addTri(vfront, ifront, front[0], front[2], front[3]); + } + + if (bcount == 4) + { + addTri(vback, iback, back[0], back[2], back[3]); + } + + break; + } + } + + // ok... here we recursively call + if (ifront.Count > 0) + { + int vcount = vfront.GetSize(); + List vertices2 = vfront.GetVertices(); + for (int i = 0; i < vertices2.Count; i++) + vertices2[i] = new float3(vertices2[i]); + int tcount = ifront.Count / 3; + + calcConvexDecomposition(vertices2, ifront, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); + } + + ifront.Clear(); + vfront.Clear(); + + if (iback.Count > 0) + { + int vcount = vback.GetSize(); + List vertices2 = vback.GetVertices(); + int tcount = iback.Count / 3; + + calcConvexDecomposition(vertices2, iback, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); + } + + iback.Clear(); + vback.Clear(); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs new file mode 100644 index 0000000000..87758b5971 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs @@ -0,0 +1,74 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class ConvexResult + { + public List HullVertices; + public List HullIndices; + + public float mHullVolume; // the volume of the convex hull. + + //public float[] OBBSides = new float[3]; // the width, height and breadth of the best fit OBB + //public float[] OBBCenter = new float[3]; // the center of the OBB + //public float[] OBBOrientation = new float[4]; // the quaternion rotation of the OBB. + //public float[] OBBTransform = new float[16]; // the 4x4 transform of the OBB. + //public float OBBVolume; // the volume of the OBB + + //public float SphereRadius; // radius and center of best fit sphere + //public float[] SphereCenter = new float[3]; + //public float SphereVolume; // volume of the best fit sphere + + public ConvexResult() + { + HullVertices = new List(); + HullIndices = new List(); + } + + public ConvexResult(List hvertices, List hindices) + { + HullVertices = hvertices; + HullIndices = hindices; + } + + public ConvexResult(ConvexResult r) + { + HullVertices = new List(r.HullVertices); + HullIndices = new List(r.HullIndices); + } + + public void Dispose() + { + HullVertices = null; + HullIndices = null; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs new file mode 100644 index 0000000000..d81df262ae --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs @@ -0,0 +1,171 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class HullResult + { + public bool Polygons = true; // true if indices represents polygons, false indices are triangles + public List OutputVertices = new List(); + public List Indices; + + // If triangles, then indices are array indexes into the vertex list. + // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. + } + + public class PHullResult + { + public List Vertices = new List(); + public List Indices = new List(); + } + + [Flags] + public enum HullFlag : int + { + QF_DEFAULT = 0, + QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons. + QF_SKIN_WIDTH = (1 << 2) // extrude hull based on this skin width + } + + public enum HullError : int + { + QE_OK, // success! + QE_FAIL // failed. + } + + public class HullDesc + { + public HullFlag Flags; // flags to use when generating the convex hull. + public List Vertices; + public float NormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + public float SkinWidth; + public uint MaxVertices; // maximum number of vertices to be considered for the hull! + public uint MaxFaces; + + public HullDesc() + { + Flags = HullFlag.QF_DEFAULT; + Vertices = new List(); + NormalEpsilon = 0.001f; + MaxVertices = 4096; + MaxFaces = 4096; + SkinWidth = 0.01f; + } + + public HullDesc(HullFlag flags, List vertices) + { + Flags = flags; + Vertices = new List(vertices); + NormalEpsilon = 0.001f; + MaxVertices = 4096; + MaxFaces = 4096; + SkinWidth = 0.01f; + } + + public bool HasHullFlag(HullFlag flag) + { + return (Flags & flag) != 0; + } + + public void SetHullFlag(HullFlag flag) + { + Flags |= flag; + } + + public void ClearHullFlag(HullFlag flag) + { + Flags &= ~flag; + } + } + + public class ConvexH + { + public struct HalfEdge + { + public short ea; // the other half of the edge (index into edges list) + public byte v; // the vertex at the start of this edge (index into vertices list) + public byte p; // the facet on which this edge lies (index into facets list) + + public HalfEdge(short _ea, byte _v, byte _p) + { + ea = _ea; + v = _v; + p = _p; + } + + public HalfEdge(HalfEdge e) + { + ea = e.ea; + v = e.v; + p = e.p; + } + } + + public List vertices = new List(); + public List edges = new List(); + public List facets = new List(); + + public ConvexH(int vertices_size, int edges_size, int facets_size) + { + vertices = new List(vertices_size); + edges = new List(edges_size); + facets = new List(facets_size); + } + } + + public class VertFlag + { + public byte planetest; + public byte junk; + public byte undermap; + public byte overmap; + } + + public class EdgeFlag + { + public byte planetest; + public byte fixes; + public short undermap; + public short overmap; + } + + public class PlaneFlag + { + public byte undermap; + public byte overmap; + } + + public class Coplanar + { + public ushort ea; + public byte v0; + public byte v1; + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs new file mode 100644 index 0000000000..1119a75e91 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs @@ -0,0 +1,99 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class HullTriangle : int3 + { + public int3 n = new int3(); + public int id; + public int vmax; + public float rise; + private List tris; + + public HullTriangle(int a, int b, int c, List tris) + : base(a, b, c) + { + this.tris = tris; + + n = new int3(-1, -1, -1); + id = tris.Count; + tris.Add(this); + vmax = -1; + rise = 0.0f; + } + + public void Dispose() + { + Debug.Assert(tris[id] == this); + tris[id] = null; + } + + public int neib(int a, int b) + { + int i; + + for (i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + if ((this)[i] == a && (this)[i1] == b) + return n[i2]; + if ((this)[i] == b && (this)[i1] == a) + return n[i2]; + } + + Debug.Assert(false); + return -1; + } + + public void setneib(int a, int b, int value) + { + int i; + + for (i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + if ((this)[i] == a && (this)[i1] == b) + { + n[i2] = value; + return; + } + if ((this)[i] == b && (this)[i1] == a) + { + n[i2] = value; + return; + } + } + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs new file mode 100644 index 0000000000..c9ccfe2c61 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs @@ -0,0 +1,1868 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public static class HullUtils + { + public static int argmin(float[] a, int n) + { + int r = 0; + for (int i = 1; i < n; i++) + { + if (a[i] < a[r]) + { + r = i; + } + } + return r; + } + + public static float clampf(float a) + { + return Math.Min(1.0f, Math.Max(0.0f, a)); + } + + public static float Round(float a, float precision) + { + return (float)Math.Floor(0.5f + a / precision) * precision; + } + + public static float Interpolate(float f0, float f1, float alpha) + { + return f0 * (1 - alpha) + f1 * alpha; + } + + public static void Swap(ref T a, ref T b) + { + T tmp = a; + a = b; + b = tmp; + } + + public static bool above(List vertices, int3 t, float3 p, float epsilon) + { + float3 vtx = vertices[t.x]; + float3 n = TriNormal(vtx, vertices[t.y], vertices[t.z]); + return (float3.dot(n, p - vtx) > epsilon); // EPSILON??? + } + + public static int hasedge(int3 t, int a, int b) + { + for (int i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + if (t[i] == a && t[i1] == b) + return 1; + } + return 0; + } + + public static bool hasvert(int3 t, int v) + { + return (t[0] == v || t[1] == v || t[2] == v); + } + + public static int shareedge(int3 a, int3 b) + { + int i; + for (i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + if (hasedge(a, b[i1], b[i]) != 0) + return 1; + } + return 0; + } + + public static void b2bfix(HullTriangle s, HullTriangle t, List tris) + { + int i; + for (i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + int a = (s)[i1]; + int b = (s)[i2]; + Debug.Assert(tris[s.neib(a, b)].neib(b, a) == s.id); + Debug.Assert(tris[t.neib(a, b)].neib(b, a) == t.id); + tris[s.neib(a, b)].setneib(b, a, t.neib(b, a)); + tris[t.neib(b, a)].setneib(a, b, s.neib(a, b)); + } + } + + public static void removeb2b(HullTriangle s, HullTriangle t, List tris) + { + b2bfix(s, t, tris); + s.Dispose(); + t.Dispose(); + } + + public static void checkit(HullTriangle t, List tris) + { + int i; + Debug.Assert(tris[t.id] == t); + for (i = 0; i < 3; i++) + { + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + int a = (t)[i1]; + int b = (t)[i2]; + Debug.Assert(a != b); + Debug.Assert(tris[t.n[i]].neib(b, a) == t.id); + } + } + + public static void extrude(HullTriangle t0, int v, List tris) + { + int3 t = t0; + int n = tris.Count; + HullTriangle ta = new HullTriangle(v, t[1], t[2], tris); + ta.n = new int3(t0.n[0], n + 1, n + 2); + tris[t0.n[0]].setneib(t[1], t[2], n + 0); + HullTriangle tb = new HullTriangle(v, t[2], t[0], tris); + tb.n = new int3(t0.n[1], n + 2, n + 0); + tris[t0.n[1]].setneib(t[2], t[0], n + 1); + HullTriangle tc = new HullTriangle(v, t[0], t[1], tris); + tc.n = new int3(t0.n[2], n + 0, n + 1); + tris[t0.n[2]].setneib(t[0], t[1], n + 2); + checkit(ta, tris); + checkit(tb, tris); + checkit(tc, tris); + if (hasvert(tris[ta.n[0]], v)) + removeb2b(ta, tris[ta.n[0]], tris); + if (hasvert(tris[tb.n[0]], v)) + removeb2b(tb, tris[tb.n[0]], tris); + if (hasvert(tris[tc.n[0]], v)) + removeb2b(tc, tris[tc.n[0]], tris); + t0.Dispose(); + } + + public static HullTriangle extrudable(float epsilon, List tris) + { + int i; + HullTriangle t = null; + for (i = 0; i < tris.Count; i++) + { + if (t == null || (tris.Count > i && (object)tris[i] != null && t.rise < tris[i].rise)) + { + t = tris[i]; + } + } + return (t.rise > epsilon) ? t : null; + } + + public static Quaternion RotationArc(float3 v0, float3 v1) + { + Quaternion q = new Quaternion(); + v0 = float3.normalize(v0); // Comment these two lines out if you know its not needed. + v1 = float3.normalize(v1); // If vector is already unit length then why do it again? + float3 c = float3.cross(v0, v1); + float d = float3.dot(v0, v1); + if (d <= -1.0f) // 180 about x axis + { + return new Quaternion(1f, 0f, 0f, 0f); + } + float s = (float)Math.Sqrt((1 + d) * 2f); + q.x = c.x / s; + q.y = c.y / s; + q.z = c.z / s; + q.w = s / 2.0f; + return q; + } + + public static float3 PlaneLineIntersection(Plane plane, float3 p0, float3 p1) + { + // returns the point where the line p0-p1 intersects the plane n&d + float3 dif = p1 - p0; + float dn = float3.dot(plane.normal, dif); + float t = -(plane.dist + float3.dot(plane.normal, p0)) / dn; + return p0 + (dif * t); + } + + public static float3 LineProject(float3 p0, float3 p1, float3 a) + { + float3 w = new float3(); + w = p1 - p0; + float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); + return p0 + w * t; + } + + public static float3 PlaneProject(Plane plane, float3 point) + { + return point - plane.normal * (float3.dot(point, plane.normal) + plane.dist); + } + + public static float LineProjectTime(float3 p0, float3 p1, float3 a) + { + float3 w = new float3(); + w = p1 - p0; + float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); + return t; + } + + public static float3 ThreePlaneIntersection(Plane p0, Plane p1, Plane p2) + { + float3x3 mp = float3x3.Transpose(new float3x3(p0.normal, p1.normal, p2.normal)); + float3x3 mi = float3x3.Inverse(mp); + float3 b = new float3(p0.dist, p1.dist, p2.dist); + return -b * mi; + } + + public static bool PolyHit(List vert, float3 v0, float3 v1) + { + float3 impact = new float3(); + float3 normal = new float3(); + return PolyHit(vert, v0, v1, out impact, out normal); + } + + public static bool PolyHit(List vert, float3 v0, float3 v1, out float3 impact) + { + float3 normal = new float3(); + return PolyHit(vert, v0, v1, out impact, out normal); + } + + public static bool PolyHit(List vert, float3 v0, float3 v1, out float3 impact, out float3 normal) + { + float3 the_point = new float3(); + + impact = null; + normal = null; + + int i; + float3 nrml = new float3(0, 0, 0); + for (i = 0; i < vert.Count; i++) + { + int i1 = (i + 1) % vert.Count; + int i2 = (i + 2) % vert.Count; + nrml = nrml + float3.cross(vert[i1] - vert[i], vert[i2] - vert[i1]); + } + + float m = float3.magnitude(nrml); + if (m == 0.0) + { + return false; + } + nrml = nrml * (1.0f / m); + float dist = -float3.dot(nrml, vert[0]); + float d0; + float d1; + if ((d0 = float3.dot(v0, nrml) + dist) < 0 || (d1 = float3.dot(v1, nrml) + dist) > 0) + { + return false; + } + + // By using the cached plane distances d0 and d1 + // we can optimize the following: + // the_point = planelineintersection(nrml,dist,v0,v1); + float a = d0 / (d0 - d1); + the_point = v0 * (1 - a) + v1 * a; + + + bool inside = true; + for (int j = 0; inside && j < vert.Count; j++) + { + // let inside = 0 if outside + float3 pp1 = new float3(); + float3 pp2 = new float3(); + float3 side = new float3(); + pp1 = vert[j]; + pp2 = vert[(j + 1) % vert.Count]; + side = float3.cross((pp2 - pp1), (the_point - pp1)); + inside = (float3.dot(nrml, side) >= 0.0); + } + if (inside) + { + if (normal != null) + { + normal = nrml; + } + if (impact != null) + { + impact = the_point; + } + } + return inside; + } + + public static bool BoxInside(float3 p, float3 bmin, float3 bmax) + { + return (p.x >= bmin.x && p.x <= bmax.x && p.y >= bmin.y && p.y <= bmax.y && p.z >= bmin.z && p.z <= bmax.z); + } + + public static bool BoxIntersect(float3 v0, float3 v1, float3 bmin, float3 bmax, float3 impact) + { + if (BoxInside(v0, bmin, bmax)) + { + impact = v0; + return true; + } + if (v0.x <= bmin.x && v1.x >= bmin.x) + { + float a = (bmin.x - v0.x) / (v1.x - v0.x); + //v.x = bmin.x; + float vy = (1 - a) * v0.y + a * v1.y; + float vz = (1 - a) * v0.z + a * v1.z; + if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) + { + impact.x = bmin.x; + impact.y = vy; + impact.z = vz; + return true; + } + } + else if (v0.x >= bmax.x && v1.x <= bmax.x) + { + float a = (bmax.x - v0.x) / (v1.x - v0.x); + //v.x = bmax.x; + float vy = (1 - a) * v0.y + a * v1.y; + float vz = (1 - a) * v0.z + a * v1.z; + if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) + { + impact.x = bmax.x; + impact.y = vy; + impact.z = vz; + return true; + } + } + if (v0.y <= bmin.y && v1.y >= bmin.y) + { + float a = (bmin.y - v0.y) / (v1.y - v0.y); + float vx = (1 - a) * v0.x + a * v1.x; + //v.y = bmin.y; + float vz = (1 - a) * v0.z + a * v1.z; + if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) + { + impact.x = vx; + impact.y = bmin.y; + impact.z = vz; + return true; + } + } + else if (v0.y >= bmax.y && v1.y <= bmax.y) + { + float a = (bmax.y - v0.y) / (v1.y - v0.y); + float vx = (1 - a) * v0.x + a * v1.x; + // vy = bmax.y; + float vz = (1 - a) * v0.z + a * v1.z; + if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) + { + impact.x = vx; + impact.y = bmax.y; + impact.z = vz; + return true; + } + } + if (v0.z <= bmin.z && v1.z >= bmin.z) + { + float a = (bmin.z - v0.z) / (v1.z - v0.z); + float vx = (1 - a) * v0.x + a * v1.x; + float vy = (1 - a) * v0.y + a * v1.y; + // v.z = bmin.z; + if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) + { + impact.x = vx; + impact.y = vy; + impact.z = bmin.z; + return true; + } + } + else if (v0.z >= bmax.z && v1.z <= bmax.z) + { + float a = (bmax.z - v0.z) / (v1.z - v0.z); + float vx = (1 - a) * v0.x + a * v1.x; + float vy = (1 - a) * v0.y + a * v1.y; + // v.z = bmax.z; + if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) + { + impact.x = vx; + impact.y = vy; + impact.z = bmax.z; + return true; + } + } + return false; + } + + public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint) + { + return DistanceBetweenLines(ustart, udir, vstart, vdir, upoint, null); + } + + public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir) + { + return DistanceBetweenLines(ustart, udir, vstart, vdir, null, null); + } + + public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint, float3 vpoint) + { + float3 cp = float3.normalize(float3.cross(udir, vdir)); + + float distu = -float3.dot(cp, ustart); + float distv = -float3.dot(cp, vstart); + float dist = (float)Math.Abs(distu - distv); + if (upoint != null) + { + Plane plane = new Plane(); + plane.normal = float3.normalize(float3.cross(vdir, cp)); + plane.dist = -float3.dot(plane.normal, vstart); + upoint = PlaneLineIntersection(plane, ustart, ustart + udir); + } + if (vpoint != null) + { + Plane plane = new Plane(); + plane.normal = float3.normalize(float3.cross(udir, cp)); + plane.dist = -float3.dot(plane.normal, ustart); + vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir); + } + return dist; + } + + public static float3 TriNormal(float3 v0, float3 v1, float3 v2) + { + // return the normal of the triangle + // inscribed by v0, v1, and v2 + float3 cp = float3.cross(v1 - v0, v2 - v1); + float m = float3.magnitude(cp); + if (m == 0) + return new float3(1, 0, 0); + return cp * (1.0f / m); + } + + public static int PlaneTest(Plane p, float3 v, float planetestepsilon) + { + float a = float3.dot(v, p.normal) + p.dist; + int flag = (a > planetestepsilon) ? (2) : ((a < -planetestepsilon) ? (1) : (0)); + return flag; + } + + public static int SplitTest(ref ConvexH convex, Plane plane, float planetestepsilon) + { + int flag = 0; + for (int i = 0; i < convex.vertices.Count; i++) + { + flag |= PlaneTest(plane, convex.vertices[i], planetestepsilon); + } + return flag; + } + + public static Quaternion VirtualTrackBall(float3 cop, float3 cor, float3 dir1, float3 dir2) + { + // routine taken from game programming gems. + // Implement track ball functionality to spin stuf on the screen + // cop center of projection + // cor center of rotation + // dir1 old mouse direction + // dir2 new mouse direction + // pretend there is a sphere around cor. Then find the points + // where dir1 and dir2 intersect that sphere. Find the + // rotation that takes the first point to the second. + float m; + // compute plane + float3 nrml = cor - cop; + float fudgefactor = 1.0f / (float3.magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop + nrml = float3.normalize(nrml); + float dist = -float3.dot(nrml, cor); + float3 u = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir1); + u = u - cor; + u = u * fudgefactor; + m = float3.magnitude(u); + if (m > 1) + { + u /= m; + } + else + { + u = u - (nrml * (float)Math.Sqrt(1 - m * m)); + } + float3 v = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir2); + v = v - cor; + v = v * fudgefactor; + m = float3.magnitude(v); + if (m > 1) + { + v /= m; + } + else + { + v = v - (nrml * (float)Math.Sqrt(1 - m * m)); + } + return RotationArc(u, v); + } + + public static bool AssertIntact(ConvexH convex, float planetestepsilon) + { + int i; + int estart = 0; + for (i = 0; i < convex.edges.Count; i++) + { + if (convex.edges[estart].p != convex.edges[i].p) + { + estart = i; + } + int inext = i + 1; + if (inext >= convex.edges.Count || convex.edges[inext].p != convex.edges[i].p) + { + inext = estart; + } + Debug.Assert(convex.edges[inext].p == convex.edges[i].p); + int nb = convex.edges[i].ea; + Debug.Assert(nb != 255); + if (nb == 255 || nb == -1) + return false; + Debug.Assert(nb != -1); + Debug.Assert(i == convex.edges[nb].ea); + } + for (i = 0; i < convex.edges.Count; i++) + { + Debug.Assert((0) == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)); + if ((0) != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)) + return false; + if (convex.edges[estart].p != convex.edges[i].p) + { + estart = i; + } + int i1 = i + 1; + if (i1 >= convex.edges.Count || convex.edges[i1].p != convex.edges[i].p) + { + i1 = estart; + } + int i2 = i1 + 1; + if (i2 >= convex.edges.Count || convex.edges[i2].p != convex.edges[i].p) + { + i2 = estart; + } + if (i == i2) // i sliced tangent to an edge and created 2 meaningless edges + continue; + float3 localnormal = TriNormal(convex.vertices[convex.edges[i].v], convex.vertices[convex.edges[i1].v], convex.vertices[convex.edges[i2].v]); + Debug.Assert(float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0); + if (float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0) + return false; + } + return true; + } + + public static ConvexH test_btbq(float planetestepsilon) + { + // back to back quads + ConvexH convex = new ConvexH(4, 8, 2); + convex.vertices[0] = new float3(0, 0, 0); + convex.vertices[1] = new float3(1, 0, 0); + convex.vertices[2] = new float3(1, 1, 0); + convex.vertices[3] = new float3(0, 1, 0); + convex.facets[0] = new Plane(new float3(0, 0, 1), 0); + convex.facets[1] = new Plane(new float3(0, 0, -1), 0); + convex.edges[0] = new ConvexH.HalfEdge(7, 0, 0); + convex.edges[1] = new ConvexH.HalfEdge(6, 1, 0); + convex.edges[2] = new ConvexH.HalfEdge(5, 2, 0); + convex.edges[3] = new ConvexH.HalfEdge(4, 3, 0); + + convex.edges[4] = new ConvexH.HalfEdge(3, 0, 1); + convex.edges[5] = new ConvexH.HalfEdge(2, 3, 1); + convex.edges[6] = new ConvexH.HalfEdge(1, 2, 1); + convex.edges[7] = new ConvexH.HalfEdge(0, 1, 1); + AssertIntact(convex, planetestepsilon); + return convex; + } + + public static ConvexH test_cube() + { + ConvexH convex = new ConvexH(8, 24, 6); + convex.vertices[0] = new float3(0, 0, 0); + convex.vertices[1] = new float3(0, 0, 1); + convex.vertices[2] = new float3(0, 1, 0); + convex.vertices[3] = new float3(0, 1, 1); + convex.vertices[4] = new float3(1, 0, 0); + convex.vertices[5] = new float3(1, 0, 1); + convex.vertices[6] = new float3(1, 1, 0); + convex.vertices[7] = new float3(1, 1, 1); + + convex.facets[0] = new Plane(new float3(-1, 0, 0), 0); + convex.facets[1] = new Plane(new float3(1, 0, 0), -1); + convex.facets[2] = new Plane(new float3(0, -1, 0), 0); + convex.facets[3] = new Plane(new float3(0, 1, 0), -1); + convex.facets[4] = new Plane(new float3(0, 0, -1), 0); + convex.facets[5] = new Plane(new float3(0, 0, 1), -1); + + convex.edges[0] = new ConvexH.HalfEdge(11, 0, 0); + convex.edges[1] = new ConvexH.HalfEdge(23, 1, 0); + convex.edges[2] = new ConvexH.HalfEdge(15, 3, 0); + convex.edges[3] = new ConvexH.HalfEdge(16, 2, 0); + + convex.edges[4] = new ConvexH.HalfEdge(13, 6, 1); + convex.edges[5] = new ConvexH.HalfEdge(21, 7, 1); + convex.edges[6] = new ConvexH.HalfEdge(9, 5, 1); + convex.edges[7] = new ConvexH.HalfEdge(18, 4, 1); + + convex.edges[8] = new ConvexH.HalfEdge(19, 0, 2); + convex.edges[9] = new ConvexH.HalfEdge(6, 4, 2); + convex.edges[10] = new ConvexH.HalfEdge(20, 5, 2); + convex.edges[11] = new ConvexH.HalfEdge(0, 1, 2); + + convex.edges[12] = new ConvexH.HalfEdge(22, 3, 3); + convex.edges[13] = new ConvexH.HalfEdge(4, 7, 3); + convex.edges[14] = new ConvexH.HalfEdge(17, 6, 3); + convex.edges[15] = new ConvexH.HalfEdge(2, 2, 3); + + convex.edges[16] = new ConvexH.HalfEdge(3, 0, 4); + convex.edges[17] = new ConvexH.HalfEdge(14, 2, 4); + convex.edges[18] = new ConvexH.HalfEdge(7, 6, 4); + convex.edges[19] = new ConvexH.HalfEdge(8, 4, 4); + + convex.edges[20] = new ConvexH.HalfEdge(10, 1, 5); + convex.edges[21] = new ConvexH.HalfEdge(5, 5, 5); + convex.edges[22] = new ConvexH.HalfEdge(12, 7, 5); + convex.edges[23] = new ConvexH.HalfEdge(1, 3, 5); + + return convex; + } + + public static ConvexH ConvexHMakeCube(float3 bmin, float3 bmax) + { + ConvexH convex = test_cube(); + convex.vertices[0] = new float3(bmin.x, bmin.y, bmin.z); + convex.vertices[1] = new float3(bmin.x, bmin.y, bmax.z); + convex.vertices[2] = new float3(bmin.x, bmax.y, bmin.z); + convex.vertices[3] = new float3(bmin.x, bmax.y, bmax.z); + convex.vertices[4] = new float3(bmax.x, bmin.y, bmin.z); + convex.vertices[5] = new float3(bmax.x, bmin.y, bmax.z); + convex.vertices[6] = new float3(bmax.x, bmax.y, bmin.z); + convex.vertices[7] = new float3(bmax.x, bmax.y, bmax.z); + + convex.facets[0] = new Plane(new float3(-1, 0, 0), bmin.x); + convex.facets[1] = new Plane(new float3(1, 0, 0), -bmax.x); + convex.facets[2] = new Plane(new float3(0, -1, 0), bmin.y); + convex.facets[3] = new Plane(new float3(0, 1, 0), -bmax.y); + convex.facets[4] = new Plane(new float3(0, 0, -1), bmin.z); + convex.facets[5] = new Plane(new float3(0, 0, 1), -bmax.z); + return convex; + } + + public static ConvexH ConvexHCrop(ref ConvexH convex, Plane slice, float planetestepsilon) + { + int i; + int vertcountunder = 0; + int vertcountover = 0; + List vertscoplanar = new List(); // existing vertex members of convex that are coplanar + List edgesplit = new List(); // existing edges that members of convex that cross the splitplane + + Debug.Assert(convex.edges.Count < 480); + + EdgeFlag[] edgeflag = new EdgeFlag[512]; + VertFlag[] vertflag = new VertFlag[256]; + PlaneFlag[] planeflag = new PlaneFlag[128]; + ConvexH.HalfEdge[] tmpunderedges = new ConvexH.HalfEdge[512]; + Plane[] tmpunderplanes = new Plane[128]; + Coplanar[] coplanaredges = new Coplanar[512]; + int coplanaredges_num = 0; + + List createdverts = new List(); + + // do the side-of-plane tests + for (i = 0; i < convex.vertices.Count; i++) + { + vertflag[i].planetest = (byte)PlaneTest(slice, convex.vertices[i], planetestepsilon); + if (vertflag[i].planetest == (0)) + { + // ? vertscoplanar.Add(i); + vertflag[i].undermap = (byte)vertcountunder++; + vertflag[i].overmap = (byte)vertcountover++; + } + else if (vertflag[i].planetest == (1)) + { + vertflag[i].undermap = (byte)vertcountunder++; + } + else + { + Debug.Assert(vertflag[i].planetest == (2)); + vertflag[i].overmap = (byte)vertcountover++; + vertflag[i].undermap = 255; // for debugging purposes + } + } + int vertcountunderold = vertcountunder; // for debugging only + + int under_edge_count = 0; + int underplanescount = 0; + int e0 = 0; + + for (int currentplane = 0; currentplane < convex.facets.Count; currentplane++) + { + int estart = e0; + int enextface = 0; + int planeside = 0; + int e1 = e0 + 1; + int vout = -1; + int vin = -1; + int coplanaredge = -1; + do + { + + if (e1 >= convex.edges.Count || convex.edges[e1].p != currentplane) + { + enextface = e1; + e1 = estart; + } + ConvexH.HalfEdge edge0 = convex.edges[e0]; + ConvexH.HalfEdge edge1 = convex.edges[e1]; + ConvexH.HalfEdge edgea = convex.edges[edge0.ea]; + + planeside |= vertflag[edge0.v].planetest; + //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { + // assert(ecop==-1); + // ecop=e; + //} + + if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (2)) + { + // both endpoints over plane + edgeflag[e0].undermap = -1; + } + else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (1)) + { + // at least one endpoint under, the other coplanar or under + + edgeflag[e0].undermap = (short)under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + if (edge0.ea < e0) + { + // connect the neighbors + Debug.Assert(edgeflag[edge0.ea].undermap != -1); + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; + } + under_edge_count++; + } + else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0)) + { + // both endpoints coplanar + // must check a 3rd point to see if UNDER + int e2 = e1 + 1; + if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane) + { + e2 = estart; + } + Debug.Assert(convex.edges[e2].p == currentplane); + ConvexH.HalfEdge edge2 = convex.edges[e2]; + if (vertflag[edge2.v].planetest == (1)) + { + + edgeflag[e0].undermap = (short)under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + tmpunderedges[under_edge_count].ea = -1; + // make sure this edge is added to the "coplanar" list + coplanaredge = under_edge_count; + vout = vertflag[edge0.v].undermap; + vin = vertflag[edge1.v].undermap; + under_edge_count++; + } + else + { + edgeflag[e0].undermap = -1; + } + } + else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2)) + { + // first is under 2nd is over + + edgeflag[e0].undermap = (short)under_edge_count; + tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + if (edge0.ea < e0) + { + Debug.Assert(edgeflag[edge0.ea].undermap != -1); + // connect the neighbors + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; + vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; + } + else + { + Plane p0 = convex.facets[edge0.p]; + Plane pa = convex.facets[edgea.p]; + createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); + //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); + //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); + vout = vertcountunder++; + } + under_edge_count++; + /// hmmm something to think about: i might be able to output this edge regarless of + // wheter or not we know v-in yet. ok i;ll try this now: + tmpunderedges[under_edge_count].v = (byte)vout; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + tmpunderedges[under_edge_count].ea = -1; + coplanaredge = under_edge_count; + under_edge_count++; + + if (vin != -1) + { + // we previously processed an edge where we came under + // now we know about vout as well + + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + + } + else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2)) + { + // first is coplanar 2nd is over + + edgeflag[e0].undermap = -1; + vout = vertflag[edge0.v].undermap; + // I hate this but i have to make sure part of this face is UNDER before ouputting this vert + int k = estart; + Debug.Assert(edge0.p == currentplane); + while (!((planeside & 1) != 0) && k < convex.edges.Count && convex.edges[k].p == edge0.p) + { + planeside |= vertflag[convex.edges[k].v].planetest; + k++; + } + if ((planeside & 1) != 0) + { + tmpunderedges[under_edge_count].v = (byte)vout; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + tmpunderedges[under_edge_count].ea = -1; + coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on + under_edge_count++; + + } + } + else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1)) + { + // first is over next is under + // new vertex!!! + Debug.Assert(vin == -1); + if (e0 < edge0.ea) + { + Plane p0 = convex.facets[edge0.p]; + Plane pa = convex.facets[edgea.p]; + createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); + //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); + //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); + vin = vertcountunder++; + } + else + { + // find the new vertex that was created by edge[edge0.ea] + int nea = edgeflag[edge0.ea].undermap; + Debug.Assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p); + vin = tmpunderedges[nea + 1].v; + Debug.Assert(vin < vertcountunder); + Debug.Assert(vin >= vertcountunderold); // for debugging only + } + if (vout != -1) + { + // we previously processed an edge where we went over + // now we know vin too + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + // output edge + tmpunderedges[under_edge_count].v = (byte)vin; + tmpunderedges[under_edge_count].p = (byte)underplanescount; + edgeflag[e0].undermap = (short)under_edge_count; + if (e0 > edge0.ea) + { + Debug.Assert(edgeflag[edge0.ea].undermap != -1); + // connect the neighbors + tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; + tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; + } + Debug.Assert(edgeflag[e0].undermap == under_edge_count); + under_edge_count++; + } + else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0)) + { + // first is over next is coplanar + + edgeflag[e0].undermap = -1; + vin = vertflag[edge1.v].undermap; + Debug.Assert(vin != -1); + if (vout != -1) + { + // we previously processed an edge where we came under + // now we know both endpoints + // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! + } + + } + else + { + Debug.Assert(false); + } + + + e0 = e1; + e1++; // do the modulo at the beginning of the loop + + } while (e0 != estart); + e0 = enextface; + if ((planeside & 1) != 0) + { + planeflag[currentplane].undermap = (byte)underplanescount; + tmpunderplanes[underplanescount] = convex.facets[currentplane]; + underplanescount++; + } + else + { + planeflag[currentplane].undermap = 0; + } + if (vout >= 0 && (planeside & 1) != 0) + { + Debug.Assert(vin >= 0); + Debug.Assert(coplanaredge >= 0); + Debug.Assert(coplanaredge != 511); + coplanaredges[coplanaredges_num].ea = (ushort)coplanaredge; + coplanaredges[coplanaredges_num].v0 = (byte)vin; + coplanaredges[coplanaredges_num].v1 = (byte)vout; + coplanaredges_num++; + } + } + + // add the new plane to the mix: + if (coplanaredges_num > 0) + { + tmpunderplanes[underplanescount++] = slice; + } + for (i = 0; i < coplanaredges_num - 1; i++) + { + if (coplanaredges[i].v1 != coplanaredges[i + 1].v0) + { + int j = 0; + for (j = i + 2; j < coplanaredges_num; j++) + { + if (coplanaredges[i].v1 == coplanaredges[j].v0) + { + Coplanar tmp = coplanaredges[i + 1]; + coplanaredges[i + 1] = coplanaredges[j]; + coplanaredges[j] = tmp; + break; + } + } + if (j >= coplanaredges_num) + { + Debug.Assert(j < coplanaredges_num); + return null; + } + } + } + + ConvexH punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount); + ConvexH under = punder; + + { + int k = 0; + for (i = 0; i < convex.vertices.Count; i++) + { + if (vertflag[i].planetest != (2)) + { + under.vertices[k++] = convex.vertices[i]; + } + } + i = 0; + while (k < vertcountunder) + { + under.vertices[k++] = createdverts[i++]; + } + Debug.Assert(i == createdverts.Count); + } + + for (i = 0; i < coplanaredges_num; i++) + { + ConvexH.HalfEdge edge = under.edges[under_edge_count + i]; + edge.p = (byte)(underplanescount - 1); + edge.ea = (short)coplanaredges[i].ea; + edge.v = (byte)coplanaredges[i].v0; + under.edges[under_edge_count + i] = edge; + + tmpunderedges[coplanaredges[i].ea].ea = (short)(under_edge_count + i); + } + + under.edges = new List(tmpunderedges); + under.facets = new List(tmpunderplanes); + return punder; + } + + public static ConvexH ConvexHDup(ConvexH src) + { + ConvexH dst = new ConvexH(src.vertices.Count, src.edges.Count, src.facets.Count); + dst.vertices = new List(src.vertices.Count); + foreach (float3 f in src.vertices) + dst.vertices.Add(new float3(f)); + dst.edges = new List(src.edges.Count); + foreach (ConvexH.HalfEdge e in src.edges) + dst.edges.Add(new ConvexH.HalfEdge(e)); + dst.facets = new List(src.facets.Count); + foreach (Plane p in src.facets) + dst.facets.Add(new Plane(p)); + return dst; + } + + public static int candidateplane(List planes, int planes_count, ConvexH convex, float epsilon) + { + int p = 0; + float md = 0; + int i; + for (i = 0; i < planes_count; i++) + { + float d = 0; + for (int j = 0; j < convex.vertices.Count; j++) + { + d = Math.Max(d, float3.dot(convex.vertices[j], planes[i].normal) + planes[i].dist); + } + if (i == 0 || d > md) + { + p = i; + md = d; + } + } + return (md > epsilon) ? p : -1; + } + + public static float3 orth(float3 v) + { + float3 a = float3.cross(v, new float3(0f, 0f, 1f)); + float3 b = float3.cross(v, new float3(0f, 1f, 0f)); + return float3.normalize((float3.magnitude(a) > float3.magnitude(b)) ? a : b); + } + + public static int maxdir(List p, int count, float3 dir) + { + Debug.Assert(count != 0); + int m = 0; + float currDotm = float3.dot(p[0], dir); + for (int i = 1; i < count; i++) + { + float currDoti = float3.dot(p[i], dir); + if (currDoti > currDotm) + { + currDotm = currDoti; + m = i; + } + } + return m; + } + + public static int maxdirfiltered(List p, int count, float3 dir, byte[] allow) + { + //Debug.Assert(count != 0); + int m = 0; + float currDotm = float3.dot(p[0], dir); + float currDoti; + + while (allow[m] == 0) + m++; + + for (int i = 1; i < count; i++) + { + if (allow[i] != 0) + { + currDoti = float3.dot(p[i], dir); + if (currDoti > currDotm) + { + currDotm = currDoti; + m = i; + } + } + } + //Debug.Assert(m != -1); + return m; + } + + public static int maxdirsterid(List p, int count, float3 dir, byte[] allow) + { + int m = -1; + while (m == -1) + { + m = maxdirfiltered(p, count, dir, allow); + if (allow[m] == 3) + return m; + float3 u = orth(dir); + float3 v = float3.cross(u, dir); + int ma = -1; + for (float x = 0.0f; x <= 360.0f; x += 45.0f) + { + int mb; + { + float s = (float)Math.Sin((3.14159264f / 180.0f) * (x)); + float c = (float)Math.Cos((3.14159264f / 180.0f) * (x)); + mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); + } + if (ma == m && mb == m) + { + allow[m] = 3; + return m; + } + if (ma != -1 && ma != mb) // Yuck - this is really ugly + { + int mc = ma; + for (float xx = x - 40.0f; xx <= x; xx += 5.0f) + { + float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx)); + float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx)); + int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); + if (mc == m && md == m) + { + allow[m] = 3; + return m; + } + mc = md; + } + } + ma = mb; + } + allow[m] = 0; + m = -1; + } + + Debug.Assert(false); + return m; + } + + public static int4 FindSimplex(List verts, byte[] allow) + { + float3[] basis = new float3[3]; + basis[0] = new float3(0.01f, 0.02f, 1.0f); + int p0 = maxdirsterid(verts, verts.Count, basis[0], allow); + int p1 = maxdirsterid(verts, verts.Count, -basis[0], allow); + basis[0] = verts[p0] - verts[p1]; + if (p0 == p1 || basis[0] == new float3(0, 0, 0)) + return new int4(-1, -1, -1, -1); + basis[1] = float3.cross(new float3(1, 0.02f, 0), basis[0]); + basis[2] = float3.cross(new float3(-0.02f, 1, 0), basis[0]); + basis[1] = float3.normalize((float3.magnitude(basis[1]) > float3.magnitude(basis[2])) ? basis[1] : basis[2]); + int p2 = maxdirsterid(verts, verts.Count, basis[1], allow); + if (p2 == p0 || p2 == p1) + { + p2 = maxdirsterid(verts, verts.Count, -basis[1], allow); + } + if (p2 == p0 || p2 == p1) + return new int4(-1, -1, -1, -1); + basis[1] = verts[p2] - verts[p0]; + basis[2] = float3.normalize(float3.cross(basis[1], basis[0])); + int p3 = maxdirsterid(verts, verts.Count, basis[2], allow); + if (p3 == p0 || p3 == p1 || p3 == p2) + p3 = maxdirsterid(verts, verts.Count, -basis[2], allow); + if (p3 == p0 || p3 == p1 || p3 == p2) + return new int4(-1, -1, -1, -1); + Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); + if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) + { + Swap(ref p2, ref p3); + } + return new int4(p0, p1, p2, p3); + } + + public static float GetDist(float px, float py, float pz, float3 p2) + { + float dx = px - p2.x; + float dy = py - p2.y; + float dz = pz - p2.z; + + return dx * dx + dy * dy + dz * dz; + } + + public static void ReleaseHull(PHullResult result) + { + if (result.Indices != null) + result.Indices = null; + if (result.Vertices != null) + result.Vertices = null; + } + + public static int calchullgen(List verts, int vlimit, List tris) + { + if (verts.Count < 4) + return 0; + if (vlimit == 0) + vlimit = 1000000000; + int j; + float3 bmin = new float3(verts[0]); + float3 bmax = new float3(verts[0]); + List isextreme = new List(verts.Count); + byte[] allow = new byte[verts.Count]; + for (j = 0; j < verts.Count; j++) + { + allow[j] = 1; + isextreme.Add(0); + bmin = float3.VectorMin(bmin, verts[j]); + bmax = float3.VectorMax(bmax, verts[j]); + } + float epsilon = float3.magnitude(bmax - bmin) * 0.001f; + + int4 p = FindSimplex(verts, allow); + if (p.x == -1) // simplex failed + return 0; + + float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point + HullTriangle t0 = new HullTriangle(p[2], p[3], p[1], tris); + t0.n = new int3(2, 3, 1); + HullTriangle t1 = new HullTriangle(p[3], p[2], p[0], tris); + t1.n = new int3(3, 2, 0); + HullTriangle t2 = new HullTriangle(p[0], p[1], p[3], tris); + t2.n = new int3(0, 1, 3); + HullTriangle t3 = new HullTriangle(p[1], p[0], p[2], tris); + t3.n = new int3(1, 0, 2); + isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1; + checkit(t0, tris); + checkit(t1, tris); + checkit(t2, tris); + checkit(t3, tris); + + for (j = 0; j < tris.Count; j++) + { + HullTriangle t = tris[j]; + Debug.Assert((object)t != null); + Debug.Assert(t.vmax < 0); + float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); + t.vmax = maxdirsterid(verts, verts.Count, n, allow); + t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); + } + HullTriangle te; + vlimit -= 4; + while (vlimit > 0 && (te = extrudable(epsilon, tris)) != null) + { + int3 ti = te; + int v = te.vmax; + Debug.Assert(isextreme[v] == 0); // wtf we've already done this vertex + isextreme[v] = 1; + //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already + j = tris.Count; + while (j-- != 0) + { + if (tris.Count <= j || (object)tris[j] == null) + continue; + int3 t = tris[j]; + if (above(verts, t, verts[v], 0.01f * epsilon)) + { + extrude(tris[j], v, tris); + } + } + // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle + j = tris.Count; + while (j-- != 0) + { + if (tris.Count <= j || (object)tris[j] == null) + continue; + if (!hasvert(tris[j], v)) + break; + int3 nt = tris[j]; + if (above(verts, nt, center, 0.01f * epsilon) || float3.magnitude(float3.cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f) + { + HullTriangle nb = tris[tris[j].n[0]]; + Debug.Assert(nb != null); + Debug.Assert(!hasvert(nb, v)); + Debug.Assert(nb.id < j); + extrude(nb, v, tris); + j = tris.Count; + } + } + j = tris.Count; + while (j-- != 0) + { + HullTriangle t = tris[j]; + if (t == null) + continue; + if (t.vmax >= 0) + break; + float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); + t.vmax = maxdirsterid(verts, verts.Count, n, allow); + if (isextreme[t.vmax] != 0) + { + t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate. + } + else + { + t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); + } + } + vlimit--; + } + return 1; + } + + public static bool calchull(List verts, out List tris_out, int vlimit, List tris) + { + tris_out = null; + + int rc = calchullgen(verts, vlimit, tris); + if (rc == 0) + return false; + List ts = new List(); + for (int i = 0; i < tris.Count; i++) + { + if ((object)tris[i] != null) + { + for (int j = 0; j < 3; j++) + ts.Add((tris[i])[j]); + tris[i] = null; + } + } + + tris_out = ts; + tris.Clear(); + return true; + } + + public static int calchullpbev(List verts, int vlimit, out List planes, float bevangle, List tris) + { + int i; + int j; + planes = new List(); + int rc = calchullgen(verts, vlimit, tris); + if (rc == 0) + return 0; + for (i = 0; i < tris.Count; i++) + { + if (tris[i] != null) + { + Plane p = new Plane(); + HullTriangle t = tris[i]; + p.normal = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); + p.dist = -float3.dot(p.normal, verts[(t)[0]]); + planes.Add(p); + for (j = 0; j < 3; j++) + { + if (t.n[j] < t.id) + continue; + HullTriangle s = tris[t.n[j]]; + float3 snormal = TriNormal(verts[(s)[0]], verts[(s)[1]], verts[(s)[2]]); + if (float3.dot(snormal, p.normal) >= Math.Cos(bevangle * (3.14159264f / 180.0f))) + continue; + float3 n = float3.normalize(snormal + p.normal); + planes.Add(new Plane(n, -float3.dot(n, verts[maxdir(verts, verts.Count, n)]))); + } + } + } + + tris.Clear(); + return 1; + } + + public static int overhull(List planes, List verts, int maxplanes, out List verts_out, out List faces_out, float inflate) + { + verts_out = null; + faces_out = null; + + int i; + int j; + if (verts.Count < 4) + return 0; + maxplanes = Math.Min(maxplanes, planes.Count); + float3 bmin = new float3(verts[0]); + float3 bmax = new float3(verts[0]); + for (i = 0; i < verts.Count; i++) + { + bmin = float3.VectorMin(bmin, verts[i]); + bmax = float3.VectorMax(bmax, verts[i]); + } + // float diameter = magnitude(bmax-bmin); + // inflate *=diameter; // RELATIVE INFLATION + bmin -= new float3(inflate, inflate, inflate); + bmax += new float3(inflate, inflate, inflate); + for (i = 0; i < planes.Count; i++) + { + planes[i].dist -= inflate; + } + float3 emin = new float3(bmin); + float3 emax = new float3(bmax); + float epsilon = float3.magnitude(emax - emin) * 0.025f; + float planetestepsilon = float3.magnitude(emax - emin) * (0.001f); + // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. + // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter)); + ConvexH c = ConvexHMakeCube(new float3(bmin), new float3(bmax)); + int k; + while (maxplanes-- != 0 && (k = candidateplane(planes, planes.Count, c, epsilon)) >= 0) + { + ConvexH tmp = c; + c = ConvexHCrop(ref tmp, planes[k], planetestepsilon); + if (c == null) // might want to debug this case better!!! + { + c = tmp; + break; + } + if (AssertIntact(c, planetestepsilon) == false) // might want to debug this case better too!!! + { + c = tmp; + break; + } + tmp.edges = null; + tmp.facets = null; + tmp.vertices = null; + } + + Debug.Assert(AssertIntact(c, planetestepsilon)); + //return c; + //C++ TO C# CONVERTER TODO TASK: The memory management function 'malloc' has no equivalent in C#: + faces_out = new List(); //(int)malloc(sizeof(int) * (1 + c.facets.Count + c.edges.Count)); // new int[1+c->facets.count+c->edges.count]; + int faces_count_out = 0; + i = 0; + faces_out[faces_count_out++] = -1; + k = 0; + while (i < c.edges.Count) + { + j = 1; + while (j + i < c.edges.Count && c.edges[i].p == c.edges[i + j].p) + { + j++; + } + faces_out[faces_count_out++] = j; + while (j-- != 0) + { + faces_out[faces_count_out++] = c.edges[i].v; + i++; + } + k++; + } + faces_out[0] = k; // number of faces. + Debug.Assert(k == c.facets.Count); + Debug.Assert(faces_count_out == 1 + c.facets.Count + c.edges.Count); + verts_out = c.vertices; // new float3[c->vertices.count]; + int verts_count_out = c.vertices.Count; + for (i = 0; i < c.vertices.Count; i++) + { + verts_out[i] = new float3(c.vertices[i]); + } + + c.edges = null; + c.facets = null; + c.vertices = null; + return 1; + } + + public static int overhullv(List verts, int maxplanes, out List verts_out, out List faces_out, float inflate, float bevangle, int vlimit, List tris) + { + verts_out = null; + faces_out = null; + + if (verts.Count == 0) + return 0; + List planes = new List(); + int rc = calchullpbev(verts, vlimit, out planes, bevangle, tris); + if (rc == 0) + return 0; + return overhull(planes, verts, maxplanes, out verts_out, out faces_out, inflate); + } + + public static void addPoint(ref uint vcount, List p, float x, float y, float z) + { + p.Add(new float3(x, y, z)); + vcount++; + } + + public static bool ComputeHull(List vertices, ref PHullResult result, int vlimit, float inflate) + { + List tris = new List(); + List faces; + List verts_out; + + if (inflate == 0.0f) + { + List tris_out; + bool ret = calchull(vertices, out tris_out, vlimit, tris); + if (ret == false) + return false; + + result.Indices = tris_out; + result.Vertices = vertices; + return true; + } + else + { + int ret = overhullv(vertices, 35, out verts_out, out faces, inflate, 120.0f, vlimit, tris); + if (ret == 0) + return false; + + List tris2 = new List(); + int n = faces[0]; + int k = 1; + for (int i = 0; i < n; i++) + { + int pn = faces[k++]; + for (int j = 2; j < pn; j++) + tris2.Add(new int3(faces[k], faces[k + j - 1], faces[k + j])); + k += pn; + } + Debug.Assert(tris2.Count == faces.Count - 1 - (n * 3)); + + result.Indices = new List(tris2.Count * 3); + for (int i = 0; i < tris2.Count; i++) + { + result.Indices.Add(tris2[i].x); + result.Indices.Add(tris2[i].y); + result.Indices.Add(tris2[i].z); + } + result.Vertices = verts_out; + + return true; + } + } + + private static bool CleanupVertices(List svertices, out List vertices, float normalepsilon, out float3 scale) + { + const float EPSILON = 0.000001f; + + vertices = new List(); + scale = new float3(1f, 1f, 1f); + + if (svertices.Count == 0) + return false; + + uint vcount = 0; + + float[] recip = new float[3]; + + float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; + float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; + + for (int i = 0; i < svertices.Count; i++) + { + float3 p = svertices[i]; + + for (int j = 0; j < 3; j++) + { + if (p[j] < bmin[j]) + bmin[j] = p[j]; + if (p[j] > bmax[j]) + bmax[j] = p[j]; + } + } + + float dx = bmax[0] - bmin[0]; + float dy = bmax[1] - bmin[1]; + float dz = bmax[2] - bmin[2]; + + float3 center = new float3(); + + center.x = dx * 0.5f + bmin[0]; + center.y = dy * 0.5f + bmin[1]; + center.z = dz * 0.5f + bmin[2]; + + if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svertices.Count < 3) + { + float len = Single.MaxValue; + + if (dx > EPSILON && dx < len) + len = dx; + if (dy > EPSILON && dy < len) + len = dy; + if (dz > EPSILON && dz < len) + len = dz; + + if (len == Single.MaxValue) + { + dx = dy = dz = 0.01f; // one centimeter + } + else + { + if (dx < EPSILON) // 1/5th the shortest non-zero edge. + dx = len * 0.05f; + if (dy < EPSILON) + dy = len * 0.05f; + if (dz < EPSILON) + dz = len * 0.05f; + } + + float x1 = center[0] - dx; + float x2 = center[0] + dx; + + float y1 = center[1] - dy; + float y2 = center[1] + dy; + + float z1 = center[2] - dz; + float z2 = center[2] + dz; + + addPoint(ref vcount, vertices, x1, y1, z1); + addPoint(ref vcount, vertices, x2, y1, z1); + addPoint(ref vcount, vertices, x2, y2, z1); + addPoint(ref vcount, vertices, x1, y2, z1); + addPoint(ref vcount, vertices, x1, y1, z2); + addPoint(ref vcount, vertices, x2, y1, z2); + addPoint(ref vcount, vertices, x2, y2, z2); + addPoint(ref vcount, vertices, x1, y2, z2); + + return true; // return cube + } + else + { + scale.x = dx; + scale.y = dy; + scale.z = dz; + + recip[0] = 1f / dx; + recip[1] = 1f / dy; + recip[2] = 1f / dz; + + center.x *= recip[0]; + center.y *= recip[1]; + center.z *= recip[2]; + } + + for (int i = 0; i < svertices.Count; i++) + { + float3 p = svertices[i]; + + float px = p[0]; + float py = p[1]; + float pz = p[2]; + + px = px * recip[0]; // normalize + py = py * recip[1]; // normalize + pz = pz * recip[2]; // normalize + + if (true) + { + int j; + + for (j = 0; j < vcount; j++) + { + float3 v = vertices[j]; + + float x = v[0]; + float y = v[1]; + float z = v[2]; + + float dx1 = Math.Abs(x - px); + float dy1 = Math.Abs(y - py); + float dz1 = Math.Abs(z - pz); + + if (dx1 < normalepsilon && dy1 < normalepsilon && dz1 < normalepsilon) + { + // ok, it is close enough to the old one + // now let us see if it is further from the center of the point cloud than the one we already recorded. + // in which case we keep this one instead. + float dist1 = GetDist(px, py, pz, center); + float dist2 = GetDist(v[0], v[1], v[2], center); + + if (dist1 > dist2) + { + v.x = px; + v.y = py; + v.z = pz; + } + + break; + } + } + + if (j == vcount) + { + float3 dest = new float3(px, py, pz); + vertices.Add(dest); + vcount++; + } + } + } + + // ok..now make sure we didn't prune so many vertices it is now invalid. + if (true) + { + float[] bmin2 = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; + float[] bmax2 = { Single.MinValue, Single.MinValue, Single.MinValue }; + + for (int i = 0; i < vcount; i++) + { + float3 p = vertices[i]; + for (int j = 0; j < 3; j++) + { + if (p[j] < bmin2[j]) + bmin2[j] = p[j]; + if (p[j] > bmax2[j]) + bmax2[j] = p[j]; + } + } + + float dx2 = bmax2[0] - bmin2[0]; + float dy2 = bmax2[1] - bmin2[1]; + float dz2 = bmax2[2] - bmin2[2]; + + if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3) + { + float cx = dx2 * 0.5f + bmin2[0]; + float cy = dy2 * 0.5f + bmin2[1]; + float cz = dz2 * 0.5f + bmin2[2]; + + float len = Single.MaxValue; + + if (dx2 >= EPSILON && dx2 < len) + len = dx2; + if (dy2 >= EPSILON && dy2 < len) + len = dy2; + if (dz2 >= EPSILON && dz2 < len) + len = dz2; + + if (len == Single.MaxValue) + { + dx2 = dy2 = dz2 = 0.01f; // one centimeter + } + else + { + if (dx2 < EPSILON) // 1/5th the shortest non-zero edge. + dx2 = len * 0.05f; + if (dy2 < EPSILON) + dy2 = len * 0.05f; + if (dz2 < EPSILON) + dz2 = len * 0.05f; + } + + float x1 = cx - dx2; + float x2 = cx + dx2; + + float y1 = cy - dy2; + float y2 = cy + dy2; + + float z1 = cz - dz2; + float z2 = cz + dz2; + + vcount = 0; // add box + + addPoint(ref vcount, vertices, x1, y1, z1); + addPoint(ref vcount, vertices, x2, y1, z1); + addPoint(ref vcount, vertices, x2, y2, z1); + addPoint(ref vcount, vertices, x1, y2, z1); + addPoint(ref vcount, vertices, x1, y1, z2); + addPoint(ref vcount, vertices, x2, y1, z2); + addPoint(ref vcount, vertices, x2, y2, z2); + addPoint(ref vcount, vertices, x1, y2, z2); + + return true; + } + } + + return true; + } + + private static void BringOutYourDead(List verts, out List overts, List indices) + { + int[] used = new int[verts.Count]; + int ocount = 0; + + overts = new List(); + + for (int i = 0; i < indices.Count; i++) + { + int v = indices[i]; // original array index + + Debug.Assert(v >= 0 && v < verts.Count); + + if (used[v] != 0) // if already remapped + { + indices[i] = used[v] - 1; // index to new array + } + else + { + indices[i] = ocount; // new index mapping + + overts.Add(verts[v]); // copy old vert to new vert array + + ocount++; // increment output vert count + + Debug.Assert(ocount >= 0 && ocount <= verts.Count); + + used[v] = ocount; // assign new index remapping + } + } + } + + public static HullError CreateConvexHull(HullDesc desc, ref HullResult result) + { + HullError ret = HullError.QE_FAIL; + + PHullResult hr = new PHullResult(); + + uint vcount = (uint)desc.Vertices.Count; + if (vcount < 8) + vcount = 8; + + List vsource; + float3 scale = new float3(); + + bool ok = CleanupVertices(desc.Vertices, out vsource, desc.NormalEpsilon, out scale); // normalize point cloud, remove duplicates! + + if (ok) + { + if (true) // scale vertices back to their original size. + { + for (int i = 0; i < vsource.Count; i++) + { + float3 v = vsource[i]; + v.x *= scale[0]; + v.y *= scale[1]; + v.z *= scale[2]; + } + } + + float skinwidth = 0; + if (desc.HasHullFlag(HullFlag.QF_SKIN_WIDTH)) + skinwidth = desc.SkinWidth; + + ok = ComputeHull(vsource, ref hr, (int)desc.MaxVertices, skinwidth); + + if (ok) + { + List vscratch; + BringOutYourDead(hr.Vertices, out vscratch, hr.Indices); + + ret = HullError.QE_OK; + + if (desc.HasHullFlag(HullFlag.QF_TRIANGLES)) // if he wants the results as triangle! + { + result.Polygons = false; + result.Indices = hr.Indices; + result.OutputVertices = vscratch; + } + else + { + result.Polygons = true; + result.OutputVertices = vscratch; + + if (true) + { + List source = hr.Indices; + List dest = new List(); + for (int i = 0; i < hr.Indices.Count / 3; i++) + { + dest.Add(3); + dest.Add(source[i * 3 + 0]); + dest.Add(source[i * 3 + 1]); + dest.Add(source[i * 3 + 2]); + } + + result.Indices = dest; + } + } + } + } + + return ret; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt new file mode 100644 index 0000000000..714ae898d8 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt @@ -0,0 +1,28 @@ +ConvexDecompositionDotNet +------------------------- + +The MIT License + +Copyright (c) 2010 Intel Corporation. +All rights reserved. + +Based on the convexdecomposition library from + by John W. Ratcliff and Stan Melax. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs new file mode 100644 index 0000000000..d099676208 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs @@ -0,0 +1,99 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class Plane + { + public float3 normal = new float3(); + public float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + + public Plane(float3 n, float d) + { + normal = new float3(n); + dist = d; + } + + public Plane(Plane p) + { + normal = new float3(p.normal); + dist = p.dist; + } + + public Plane() + { + dist = 0; + } + + public void Transform(float3 position, Quaternion orientation) + { + // Transforms the plane to the space defined by the + // given position/orientation + float3 newNormal = Quaternion.Inverse(orientation) * normal; + float3 origin = Quaternion.Inverse(orientation) * (-normal * dist - position); + + normal = newNormal; + dist = -float3.dot(newNormal, origin); + } + + public override int GetHashCode() + { + return normal.GetHashCode() ^ dist.GetHashCode(); + } + + public override bool Equals(object obj) + { + Plane p = obj as Plane; + if (p == null) + return false; + + return this == p; + } + + public static bool operator ==(Plane a, Plane b) + { + return (a.normal == b.normal && a.dist == b.dist); + } + + public static bool operator !=(Plane a, Plane b) + { + return !(a == b); + } + + public static Plane PlaneFlip(Plane plane) + { + return new Plane(-plane.normal, -plane.dist); + } + + public static bool coplanar(Plane a, Plane b) + { + return (a == b || a == PlaneFlip(b)); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs new file mode 100644 index 0000000000..31f0182520 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs @@ -0,0 +1,211 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public enum PlaneTriResult : int + { + PTR_FRONT, + PTR_BACK, + PTR_SPLIT + } + + public static class PlaneTri + { + private static float DistToPt(float3 p, float4 plane) + { + return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w; + } + + private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon) + { + float d = DistToPt(p, plane); + + if ((d + epsilon) > 0f) + return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value. + + return PlaneTriResult.PTR_BACK; + } + + private static void add(float3 p, float3[] dest, ref int pcount) + { + dest[pcount++] = new float3(p); + Debug.Assert(pcount <= 4); + } + + // assumes that the points are on opposite sides of the plane! + private static void intersect(float3 p1, float3 p2, float3 split, float4 plane) + { + float dp1 = DistToPt(p1, plane); + float[] dir = new float[3]; + + dir[0] = p2[0] - p1[0]; + dir[1] = p2[1] - p1[1]; + dir[2] = p2[2] - p1[2]; + + float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; + float dot2 = dp1 - plane[3]; + + float t = -(plane[3] + dot2) / dot1; + + split.x = (dir[0] * t) + p1[0]; + split.y = (dir[1] * t) + p1[1]; + split.z = (dir[2] * t) + p1[2]; + } + + public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount) + { + fcount = 0; + bcount = 0; + + // get the three vertices of the triangle. + float3 p1 = triangle.P1; + float3 p2 = triangle.P2; + float3 p3 = triangle.P3; + + PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on + PlaneTriResult r2 = getSidePlane(p2, plane, epsilon); + PlaneTriResult r3 = getSidePlane(p3, plane, epsilon); + + if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane. + { + if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle. + { + add(p1, front, ref fcount); + add(p2, front, ref fcount); + add(p3, front, ref fcount); + } + else + { + add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle. + add(p2, back, ref bcount); + add(p3, back, ref bcount); + } + return r1; // if all three points are on the same side of the plane return result + } + + // ok.. we need to split the triangle at the plane. + + // First test ray segment P1 to P2 + if (r1 == r2) // if these are both on the same side... + { + if (r1 == PlaneTriResult.PTR_FRONT) + { + add(p1, front, ref fcount); + add(p2, front, ref fcount); + } + else + { + add(p1, back, ref bcount); + add(p2, back, ref bcount); + } + } + else + { + float3 split = new float3(); + intersect(p1, p2, split, plane); + + if (r1 == PlaneTriResult.PTR_FRONT) + { + + add(p1, front, ref fcount); + add(split, front, ref fcount); + + add(split, back, ref bcount); + add(p2, back, ref bcount); + + } + else + { + add(p1, back, ref bcount); + add(split, back, ref bcount); + + add(split, front, ref fcount); + add(p2, front, ref fcount); + } + + } + + // Next test ray segment P2 to P3 + if (r2 == r3) // if these are both on the same side... + { + if (r3 == PlaneTriResult.PTR_FRONT) + { + add(p3, front, ref fcount); + } + else + { + add(p3, back, ref bcount); + } + } + else + { + float3 split = new float3(); // split the point + intersect(p2, p3, split, plane); + + if (r3 == PlaneTriResult.PTR_FRONT) + { + add(split, front, ref fcount); + add(split, back, ref bcount); + + add(p3, front, ref fcount); + } + else + { + add(split, front, ref fcount); + add(split, back, ref bcount); + + add(p3, back, ref bcount); + } + } + + // Next test ray segment P3 to P1 + if (r3 != r1) // if these are both on the same side... + { + float3 split = new float3(); // split the point + intersect(p3, p1, split, plane); + + if (r1 == PlaneTriResult.PTR_FRONT) + { + add(split, front, ref fcount); + add(split, back, ref bcount); + } + else + { + add(split, front, ref fcount); + add(split, back, ref bcount); + } + } + + return PlaneTriResult.PTR_SPLIT; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs new file mode 100644 index 0000000000..4285e8cabf --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("ConvexDecompositionDotNet")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("Intel Corporation")] +[assembly: AssemblyProduct("ConvexDecompositionDotNet")] +[assembly: AssemblyCopyright("Copyright © Intel Corporation 2010")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("2a1c9467-1a17-4c8d-bf9f-4b4d86dd0cbb")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs new file mode 100644 index 0000000000..0ba8f17a9c --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs @@ -0,0 +1,209 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class Quaternion : float4 + { + public Quaternion() + { + x = y = z = 0.0f; + w = 1.0f; + } + + public Quaternion(float3 v, float t) + { + v = float3.normalize(v); + w = (float)Math.Cos(t / 2.0f); + v = v * (float)Math.Sin(t / 2.0f); + x = v.x; + y = v.y; + z = v.z; + } + + public Quaternion(float _x, float _y, float _z, float _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + public float angle() + { + return (float)Math.Acos(w) * 2.0f; + } + + public float3 axis() + { + float3 a = new float3(x, y, z); + if (Math.Abs(angle()) < 0.0000001f) + return new float3(1f, 0f, 0f); + return a * (1 / (float)Math.Sin(angle() / 2.0f)); + } + + public float3 xdir() + { + return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)); + } + + public float3 ydir() + { + return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)); + } + + public float3 zdir() + { + return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)); + } + + public float3x3 getmatrix() + { + return new float3x3(xdir(), ydir(), zdir()); + } + + public static implicit operator float3x3(Quaternion q) + { + return q.getmatrix(); + } + + public static Quaternion operator *(Quaternion a, Quaternion b) + { + Quaternion c = new Quaternion(); + c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; + c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; + c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; + c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; + return c; + } + + public static float3 operator *(Quaternion q, float3 v) + { + // The following is equivalent to: + //return (q.getmatrix() * v); + float qx2 = q.x * q.x; + float qy2 = q.y * q.y; + float qz2 = q.z * q.z; + + float qxqy = q.x * q.y; + float qxqz = q.x * q.z; + float qxqw = q.x * q.w; + float qyqz = q.y * q.z; + float qyqw = q.y * q.w; + float qzqw = q.z * q.w; + return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z); + } + + public static Quaternion operator +(Quaternion a, Quaternion b) + { + return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); + } + + public static Quaternion operator *(Quaternion a, float b) + { + return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b); + } + + public static Quaternion normalize(Quaternion a) + { + float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z); + if (m < 0.000000001f) + { + a.w = 1; + a.x = a.y = a.z = 0; + return a; + } + return a * (1f / m); + } + + public static float dot(Quaternion a, Quaternion b) + { + return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z); + } + + public static Quaternion slerp(Quaternion a, Quaternion b, float interp) + { + if (dot(a, b) < 0.0) + { + a.w = -a.w; + a.x = -a.x; + a.y = -a.y; + a.z = -a.z; + } + float d = dot(a, b); + if (d >= 1.0) + { + return a; + } + float theta = (float)Math.Acos(d); + if (theta == 0.0f) + { + return (a); + } + return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta)); + } + + public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha) + { + return slerp(q0, q1, alpha); + } + + public static Quaternion Inverse(Quaternion q) + { + return new Quaternion(-q.x, -q.y, -q.z, q.w); + } + + public static Quaternion YawPitchRoll(float yaw, float pitch, float roll) + { + roll *= (3.14159264f / 180.0f); + yaw *= (3.14159264f / 180.0f); + pitch *= (3.14159264f / 180.0f); + return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll); + } + + public static float Yaw(Quaternion q) + { + float3 v = q.ydir(); + return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); + } + + public static float Pitch(Quaternion q) + { + float3 v = q.ydir(); + return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); + } + + public static float Roll(Quaternion q) + { + q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q; + q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q; + return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt new file mode 100644 index 0000000000..fc53ae75c8 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt @@ -0,0 +1,7 @@ +ConvexDecompositionDotNet +========================= + +A C# port of the ConvexDecomposition library by John W. Ratcliff and Stan Melax. +The original C++ version is available at . +See the blog post at +for a thorough explanation of generating convex hulls from concave meshes. diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs new file mode 100644 index 0000000000..9f06a9a23f --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs @@ -0,0 +1,265 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class Rect3d + { + public float[] mMin = new float[3]; + public float[] mMax = new float[3]; + + public Rect3d() + { + } + + public Rect3d(float[] bmin, float[] bmax) + { + mMin[0] = bmin[0]; + mMin[1] = bmin[1]; + mMin[2] = bmin[2]; + + mMax[0] = bmax[0]; + mMax[1] = bmax[1]; + mMax[2] = bmax[2]; + } + + public void SetMin(float[] bmin) + { + mMin[0] = bmin[0]; + mMin[1] = bmin[1]; + mMin[2] = bmin[2]; + } + + public void SetMax(float[] bmax) + { + mMax[0] = bmax[0]; + mMax[1] = bmax[1]; + mMax[2] = bmax[2]; + } + + public void SetMin(float x, float y, float z) + { + mMin[0] = x; + mMin[1] = y; + mMin[2] = z; + } + + public void SetMax(float x, float y, float z) + { + mMax[0] = x; + mMax[1] = y; + mMax[2] = z; + } + } + + public static class SplitPlane + { + public static bool computeSplitPlane(List vertices, List indices, ref float4 plane) + { + float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; + float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; + + for (int i = 0; i < vertices.Count; i++) + { + float3 p = vertices[i]; + + if (p[0] < bmin[0]) + bmin[0] = p[0]; + if (p[1] < bmin[1]) + bmin[1] = p[1]; + if (p[2] < bmin[2]) + bmin[2] = p[2]; + + if (p[0] > bmax[0]) + bmax[0] = p[0]; + if (p[1] > bmax[1]) + bmax[1] = p[1]; + if (p[2] > bmax[2]) + bmax[2] = p[2]; + } + + float dx = bmax[0] - bmin[0]; + float dy = bmax[1] - bmin[1]; + float dz = bmax[2] - bmin[2]; + + float laxis = dx; + + int axis = 0; + + if (dy > dx) + { + axis = 1; + laxis = dy; + } + + if (dz > dx && dz > dy) + { + axis = 2; + laxis = dz; + } + + float[] p1 = new float[3]; + float[] p2 = new float[3]; + float[] p3 = new float[3]; + + p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f; + p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f; + p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f; + + Rect3d b = new Rect3d(bmin, bmax); + + Rect3d b1 = new Rect3d(); + Rect3d b2 = new Rect3d(); + + splitRect(axis, b, b1, b2, p1); + + switch (axis) + { + case 0: + p2[1] = bmin[1]; + p2[2] = bmin[2]; + + if (dz > dy) + { + p3[1] = bmax[1]; + p3[2] = bmin[2]; + } + else + { + p3[1] = bmin[1]; + p3[2] = bmax[2]; + } + + break; + case 1: + p2[0] = bmin[0]; + p2[2] = bmin[2]; + + if (dx > dz) + { + p3[0] = bmax[0]; + p3[2] = bmin[2]; + } + else + { + p3[0] = bmin[0]; + p3[2] = bmax[2]; + } + + break; + case 2: + p2[0] = bmin[0]; + p2[1] = bmin[1]; + + if (dx > dy) + { + p3[0] = bmax[0]; + p3[1] = bmin[1]; + } + else + { + p3[0] = bmin[0]; + p3[1] = bmax[1]; + } + + break; + } + + computePlane(p1, p2, p3, plane); + + return true; + } + + internal static void computePlane(float[] A, float[] B, float[] C, float4 plane) + { + float vx = (B[0] - C[0]); + float vy = (B[1] - C[1]); + float vz = (B[2] - C[2]); + + float wx = (A[0] - B[0]); + float wy = (A[1] - B[1]); + float wz = (A[2] - B[2]); + + float vw_x = vy * wz - vz * wy; + float vw_y = vz * wx - vx * wz; + float vw_z = vx * wy - vy * wx; + + float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); + + if (mag < 0.000001f) + { + mag = 0; + } + else + { + mag = 1.0f / mag; + } + + float x = vw_x * mag; + float y = vw_y * mag; + float z = vw_z * mag; + + float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2])); + + plane.x = x; + plane.y = y; + plane.z = z; + plane.w = D; + } + + public static void splitRect(int axis, Rect3d source, Rect3d b1, Rect3d b2, float[] midpoint) + { + switch (axis) + { + case 0: + b1.SetMin(source.mMin); + b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]); + + b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]); + b2.SetMax(source.mMax); + break; + case 1: + b1.SetMin(source.mMin); + b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]); + + b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]); + b2.SetMax(source.mMax); + break; + case 2: + b1.SetMin(source.mMin); + b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]); + + b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]); + b2.SetMax(source.mMax); + break; + } + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs new file mode 100644 index 0000000000..6f17c9f711 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs @@ -0,0 +1,70 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class VertexPool + { + private List mVertices = new List(); + private Dictionary mIndices = new Dictionary(); + + public int getIndex(float3 vtx) + { + int idx; + if (mIndices.TryGetValue(vtx, out idx)) + return idx; + + idx = mVertices.Count; + mVertices.Add(vtx); + mIndices.Add(vtx, idx); + return idx; + } + + public float3 Get(int idx) + { + return mVertices[idx]; + } + + public int GetSize() + { + return mVertices.Count; + } + + public List GetVertices() + { + return mVertices; + } + + public void Clear() + { + mVertices.Clear(); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs new file mode 100644 index 0000000000..ce88fc826e --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs @@ -0,0 +1,70 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class float2 + { + public float x; + public float y; + + public float2() + { + } + + public float2(float _x, float _y) + { + x = _x; + y = _y; + } + + public float this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + } + throw new ArgumentOutOfRangeException(); + } + } + + public static float2 operator -(float2 a, float2 b) + { + return new float2(a.x - b.x, a.y - b.y); + } + + public static float2 operator +(float2 a, float2 b) + { + return new float2(a.x + b.x, a.y + b.y); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs new file mode 100644 index 0000000000..4389114848 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs @@ -0,0 +1,444 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class float3 : IEquatable + { + public float x; + public float y; + public float z; + + public float3() + { + x = 0; + y = 0; + z = 0; + } + + public float3(float _x, float _y, float _z) + { + x = _x; + y = _y; + z = _z; + } + + public float3(float3 f) + { + x = f.x; + y = f.y; + z = f.z; + } + + public float this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + } + throw new ArgumentOutOfRangeException(); + } + } + + public float Distance(float3 a) + { + float3 d = new float3(a.x - x, a.y - y, a.z - z); + return d.Length(); + } + + public float Distance2(float3 a) + { + float dx = a.x - x; + float dy = a.y - y; + float dz = a.z - z; + return dx * dx + dy * dy + dz * dz; + } + + public float Length() + { + return (float)Math.Sqrt(x * x + y * y + z * z); + } + + public float Area(float3 p1, float3 p2) + { + float A = Partial(p1); + A += p1.Partial(p2); + A += p2.Partial(this); + return A * 0.5f; + } + + public float Partial(float3 p) + { + return (x * p.y) - (p.x * y); + } + + // Given a point and a line (defined by two points), compute the closest point + // in the line. (The line is treated as infinitely long.) + public void NearestPointInLine(float3 point, float3 line0, float3 line1) + { + float3 nearestPoint = new float3(); + float3 lineDelta = line1 - line0; + + // Handle degenerate lines + if (lineDelta == float3.Zero) + { + nearestPoint = line0; + } + else + { + float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); + nearestPoint = line0 + lineDelta * delta; + } + + this.x = nearestPoint.x; + this.y = nearestPoint.y; + this.z = nearestPoint.z; + } + + // Given a point and a line segment (defined by two points), compute the closest point + // in the line. Cap the point at the endpoints of the line segment. + public void NearestPointInLineSegment(float3 point, float3 line0, float3 line1) + { + float3 nearestPoint = new float3(); + float3 lineDelta = line1 - line0; + + // Handle degenerate lines + if (lineDelta == Zero) + { + nearestPoint = line0; + } + else + { + float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); + + // Clamp the point to conform to the segment's endpoints + if (delta < 0) + delta = 0; + else if (delta > 1) + delta = 1; + + nearestPoint = line0 + lineDelta * delta; + } + + this.x = nearestPoint.x; + this.y = nearestPoint.y; + this.z = nearestPoint.z; + } + + // Given a point and a triangle (defined by three points), compute the closest point + // in the triangle. Clamp the point so it's confined to the area of the triangle. + public void NearestPointInTriangle(float3 point, float3 triangle0, float3 triangle1, float3 triangle2) + { + float3 nearestPoint = new float3(); + + float3 lineDelta0 = triangle1 - triangle0; + float3 lineDelta1 = triangle2 - triangle0; + + // Handle degenerate triangles + if ((lineDelta0 == Zero) || (lineDelta1 == Zero)) + { + nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2); + } + else if (lineDelta0 == lineDelta1) + { + nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1); + } + else + { + float3[] axis = new float3[3] { new float3(), new float3(), new float3() }; + axis[0].NearestPointInLine(triangle0, triangle1, triangle2); + axis[1].NearestPointInLine(triangle1, triangle0, triangle2); + axis[2].NearestPointInLine(triangle2, triangle0, triangle1); + + float3 axisDot = new float3(); + axisDot.x = dot(triangle0 - axis[0], point - axis[0]); + axisDot.y = dot(triangle1 - axis[1], point - axis[1]); + axisDot.z = dot(triangle2 - axis[2], point - axis[2]); + + bool bForce = true; + float bestMagnitude2 = 0; + float closeMagnitude2; + float3 closePoint = new float3(); + + if (axisDot.x < 0f) + { + closePoint.NearestPointInLineSegment(point, triangle1, triangle2); + closeMagnitude2 = point.Distance2(closePoint); + if (bForce || (bestMagnitude2 > closeMagnitude2)) + { + bForce = false; + bestMagnitude2 = closeMagnitude2; + nearestPoint = closePoint; + } + } + if (axisDot.y < 0f) + { + closePoint.NearestPointInLineSegment(point, triangle0, triangle2); + closeMagnitude2 = point.Distance2(closePoint); + if (bForce || (bestMagnitude2 > closeMagnitude2)) + { + bForce = false; + bestMagnitude2 = closeMagnitude2; + nearestPoint = closePoint; + } + } + if (axisDot.z < 0f) + { + closePoint.NearestPointInLineSegment(point, triangle0, triangle1); + closeMagnitude2 = point.Distance2(closePoint); + if (bForce || (bestMagnitude2 > closeMagnitude2)) + { + bForce = false; + bestMagnitude2 = closeMagnitude2; + nearestPoint = closePoint; + } + } + + // If bForce is true at this point, it means the nearest point lies + // inside the triangle; use the nearest-point-on-a-plane equation + if (bForce) + { + float3 normal; + + // Get the normal of the polygon (doesn't have to be a unit vector) + normal = float3.cross(lineDelta0, lineDelta1); + + float3 pointDelta = point - triangle0; + float delta = float3.dot(normal, pointDelta) / float3.dot(normal, normal); + + nearestPoint = point - normal * delta; + } + } + + this.x = nearestPoint.x; + this.y = nearestPoint.y; + this.z = nearestPoint.z; + } + + public static float3 operator +(float3 a, float3 b) + { + return new float3(a.x + b.x, a.y + b.y, a.z + b.z); + } + + public static float3 operator -(float3 a, float3 b) + { + return new float3(a.x - b.x, a.y - b.y, a.z - b.z); + } + + public static float3 operator -(float3 a, float s) + { + return new float3(a.x - s, a.y - s, a.z - s); + } + + public static float3 operator -(float3 v) + { + return new float3(-v.x, -v.y, -v.z); + } + + public static float3 operator *(float3 v, float s) + { + return new float3(v.x * s, v.y * s, v.z * s); + } + + public static float3 operator *(float s, float3 v) + { + return new float3(v.x * s, v.y * s, v.z * s); + } + + public static float3 operator *(float3 v, float3x3 m) + { + return new float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z), (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z), (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z)); + } + + public static float3 operator *(float3x3 m, float3 v) + { + return new float3(dot(m.x, v), dot(m.y, v), dot(m.z, v)); + } + + public static float3 operator /(float3 v, float s) + { + float sinv = 1.0f / s; + return new float3(v.x * sinv, v.y * sinv, v.z * sinv); + } + + public bool Equals(float3 other) + { + return this == other; + } + + public override bool Equals(object obj) + { + float3 f = obj as float3; + if (f == null) + return false; + + return this == f; + } + + public override int GetHashCode() + { + return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); + } + + public static bool operator ==(float3 a, float3 b) + { + // If both are null, or both are same instance, return true. + if (System.Object.ReferenceEquals(a, b)) + return true; + // If one is null, but not both, return false. + if (((object)a == null) || ((object)b == null)) + return false; + + return (a.x == b.x && a.y == b.y && a.z == b.z); + } + + public static bool operator !=(float3 a, float3 b) + { + return (a.x != b.x || a.y != b.y || a.z != b.z); + } + + public static float dot(float3 a, float3 b) + { + return a.x * b.x + a.y * b.y + a.z * b.z; + } + + public static float3 cmul(float3 v1, float3 v2) + { + return new float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z); + } + + public static float3 cross(float3 a, float3 b) + { + return new float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); + } + + public static float3 Interpolate(float3 v0, float3 v1, float alpha) + { + return v0 * (1 - alpha) + v1 * alpha; + } + + public static float3 Round(float3 a, int digits) + { + return new float3((float)Math.Round(a.x, digits), (float)Math.Round(a.y, digits), (float)Math.Round(a.z, digits)); + } + + public static float3 VectorMax(float3 a, float3 b) + { + return new float3(Math.Max(a.x, b.x), Math.Max(a.y, b.y), Math.Max(a.z, b.z)); + } + + public static float3 VectorMin(float3 a, float3 b) + { + return new float3(Math.Min(a.x, b.x), Math.Min(a.y, b.y), Math.Min(a.z, b.z)); + } + + public static float3 vabs(float3 v) + { + return new float3(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z)); + } + + public static float magnitude(float3 v) + { + return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + } + + public static float3 normalize(float3 v) + { + float d = magnitude(v); + if (d == 0) + d = 0.1f; + d = 1 / d; + return new float3(v.x * d, v.y * d, v.z * d); + } + + public static float3 safenormalize(float3 v) + { + if (magnitude(v) <= 0.0f) + return new float3(1, 0, 0); + else + return normalize(v); + } + + public static float Yaw(float3 v) + { + return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); + } + + public static float Pitch(float3 v) + { + return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); + } + + public float ComputePlane(float3 A, float3 B, float3 C) + { + float vx, vy, vz, wx, wy, wz, vw_x, vw_y, vw_z, mag; + + vx = (B.x - C.x); + vy = (B.y - C.y); + vz = (B.z - C.z); + + wx = (A.x - B.x); + wy = (A.y - B.y); + wz = (A.z - B.z); + + vw_x = vy * wz - vz * wy; + vw_y = vz * wx - vx * wz; + vw_z = vx * wy - vy * wx; + + mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); + + if (mag < 0.000001f) + { + mag = 0; + } + else + { + mag = 1.0f / mag; + } + + x = vw_x * mag; + y = vw_y * mag; + z = vw_z * mag; + + float D = 0.0f - ((x * A.x) + (y * A.y) + (z * A.z)); + return D; + } + + public override string ToString() + { + return String.Format("<{0}, {1}, {2}>", x, y, z); + } + + public static readonly float3 Zero = new float3(); + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs new file mode 100644 index 0000000000..76cf06347a --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs @@ -0,0 +1,195 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Diagnostics; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class float3x3 + { + public float3 x = new float3(); + public float3 y = new float3(); + public float3 z = new float3(); + + public float3x3() + { + } + + public float3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) + { + x = new float3(xx, xy, xz); + y = new float3(yx, yy, yz); + z = new float3(zx, zy, zz); + } + + public float3x3(float3 _x, float3 _y, float3 _z) + { + x = new float3(_x); + y = new float3(_y); + z = new float3(_z); + } + + public float3 this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + } + throw new ArgumentOutOfRangeException(); + } + } + + public float this[int i, int j] + { + get + { + switch (i) + { + case 0: + switch (j) + { + case 0: return x.x; + case 1: return x.y; + case 2: return x.z; + } + break; + case 1: + switch (j) + { + case 0: return y.x; + case 1: return y.y; + case 2: return y.z; + } + break; + case 2: + switch (j) + { + case 0: return z.x; + case 1: return z.y; + case 2: return z.z; + } + break; + } + throw new ArgumentOutOfRangeException(); + } + set + { + switch (i) + { + case 0: + switch (j) + { + case 0: x.x = value; return; + case 1: x.y = value; return; + case 2: x.z = value; return; + } + break; + case 1: + switch (j) + { + case 0: y.x = value; return; + case 1: y.y = value; return; + case 2: y.z = value; return; + } + break; + case 2: + switch (j) + { + case 0: z.x = value; return; + case 1: z.y = value; return; + case 2: z.z = value; return; + } + break; + } + throw new ArgumentOutOfRangeException(); + } + } + + public static float3x3 Transpose(float3x3 m) + { + return new float3x3(new float3(m.x.x, m.y.x, m.z.x), new float3(m.x.y, m.y.y, m.z.y), new float3(m.x.z, m.y.z, m.z.z)); + } + + public static float3x3 operator *(float3x3 a, float3x3 b) + { + return new float3x3(a.x * b, a.y * b, a.z * b); + } + + public static float3x3 operator *(float3x3 a, float s) + { + return new float3x3(a.x * s, a.y * s, a.z * s); + } + + public static float3x3 operator /(float3x3 a, float s) + { + float t = 1f / s; + return new float3x3(a.x * t, a.y * t, a.z * t); + } + + public static float3x3 operator +(float3x3 a, float3x3 b) + { + return new float3x3(a.x + b.x, a.y + b.y, a.z + b.z); + } + + public static float3x3 operator -(float3x3 a, float3x3 b) + { + return new float3x3(a.x - b.x, a.y - b.y, a.z - b.z); + } + + public static float Determinant(float3x3 m) + { + return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z; + } + + public static float3x3 Inverse(float3x3 a) + { + float3x3 b = new float3x3(); + float d = Determinant(a); + Debug.Assert(d != 0); + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; + int j1 = (j + 1) % 3; + int j2 = (j + 2) % 3; + + // reverse indexs i&j to take transpose + b[i, j] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d; + } + } + return b; + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs new file mode 100644 index 0000000000..fa6087679f --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs @@ -0,0 +1,170 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class float4 + { + public float x; + public float y; + public float z; + public float w; + + public float4() + { + x = 0; + y = 0; + z = 0; + w = 0; + } + + public float4(float _x, float _y, float _z, float _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + public float4(float3 v, float _w) + { + x = v.x; + y = v.y; + z = v.z; + w = _w; + } + + public float4(float4 f) + { + x = f.x; + y = f.y; + z = f.z; + w = f.w; + } + + public float this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + throw new ArgumentOutOfRangeException(); + } + } + + public float3 xyz() + { + return new float3(x, y, z); + } + + public void setxyz(float3 xyz) + { + x = xyz.x; + y = xyz.y; + z = xyz.z; + } + + public override int GetHashCode() + { + return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); + } + + public override bool Equals(object obj) + { + float4 f = obj as float4; + if (f == null) + return false; + + return this == f; + } + + public static float4 Homogenize(float3 v3) + { + return Homogenize(v3, 1.0f); + } + + //C++ TO C# CONVERTER NOTE: C# does not allow default values for parameters. Overloaded methods are inserted above. + //ORIGINAL LINE: float4 Homogenize(const float3 &v3, const float &w =1.0f) + public static float4 Homogenize(float3 v3, float w) + { + return new float4(v3.x, v3.y, v3.z, w); + } + + public static float4 cmul(float4 a, float4 b) + { + return new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); + } + + public static float4 operator +(float4 a, float4 b) + { + return new float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); + } + public static float4 operator -(float4 a, float4 b) + { + return new float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); + } + + public static float4 operator *(float4 v, float4x4 m) + { + return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w; // yes this actually works + } + + public static bool operator ==(float4 a, float4 b) + { + // If both are null, or both are same instance, return true. + if (System.Object.ReferenceEquals(a, b)) + return true; + // If one is null, but not both, return false. + if (((object)a == null) || ((object)b == null)) + return false; + + return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); + } + + public static bool operator !=(float4 a, float4 b) + { + return !(a == b); + } + + public static float4 operator *(float4 v, float s) + { + return new float4(v.x * s, v.y * s, v.z * s, v.w * s); + } + + public static float4 operator *(float s, float4 v) + { + return new float4(v.x * s, v.y * s, v.z * s, v.w * s); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs new file mode 100644 index 0000000000..7d1592f00c --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs @@ -0,0 +1,284 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class float4x4 + { + public float4 x = new float4(); + public float4 y = new float4(); + public float4 z = new float4(); + public float4 w = new float4(); + + public float4x4() + { + } + + public float4x4(float4 _x, float4 _y, float4 _z, float4 _w) + { + x = new float4(_x); + y = new float4(_y); + z = new float4(_z); + w = new float4(_w); + } + + public float4x4( + float m00, float m01, float m02, float m03, + float m10, float m11, float m12, float m13, + float m20, float m21, float m22, float m23, + float m30, float m31, float m32, float m33) + { + x = new float4(m00, m01, m02, m03); + y = new float4(m10, m11, m12, m13); + z = new float4(m20, m21, m22, m23); + w = new float4(m30, m31, m32, m33); + } + + public float4x4(float4x4 m) + { + x = new float4(m.x); + y = new float4(m.y); + z = new float4(m.z); + w = new float4(m.w); + } + + public float4 this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + throw new ArgumentOutOfRangeException(); + } + set + { + switch (i) + { + case 0: x = value; return; + case 1: y = value; return; + case 2: z = value; return; + case 3: w = value; return; + } + throw new ArgumentOutOfRangeException(); + } + } + + public override int GetHashCode() + { + return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); + } + + public override bool Equals(object obj) + { + float4x4 m = obj as float4x4; + if (m == null) + return false; + + return this == m; + } + + public static float4x4 operator *(float4x4 a, float4x4 b) + { + return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b); + } + + public static bool operator ==(float4x4 a, float4x4 b) + { + return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); + } + + public static bool operator !=(float4x4 a, float4x4 b) + { + return !(a == b); + } + + public static float4x4 Inverse(float4x4 m) + { + float4x4 d = new float4x4(); + //float dst = d.x.x; + float[] tmp = new float[12]; // temp array for pairs + float[] src = new float[16]; // array of transpose source matrix + float det; // determinant + // transpose matrix + for (int i = 0; i < 4; i++) + { + src[i] = m[i].x; + src[i + 4] = m[i].y; + src[i + 8] = m[i].z; + src[i + 12] = m[i].w; + } + // calculate pairs for first 8 elements (cofactors) + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + // calculate first 8 elements (cofactors) + d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; + d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; + d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; + d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; + d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; + d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; + d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; + d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; + d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; + d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; + d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; + d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; + d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; + d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; + d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; + d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; + // calculate pairs for second 8 elements (cofactors) + tmp[0] = src[2]*src[7]; + tmp[1] = src[3]*src[6]; + tmp[2] = src[1]*src[7]; + tmp[3] = src[3]*src[5]; + tmp[4] = src[1]*src[6]; + tmp[5] = src[2]*src[5]; + tmp[6] = src[0]*src[7]; + tmp[7] = src[3]*src[4]; + tmp[8] = src[0]*src[6]; + tmp[9] = src[2]*src[4]; + tmp[10] = src[0]*src[5]; + tmp[11] = src[1]*src[4]; + // calculate second 8 elements (cofactors) + d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; + d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; + d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; + d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; + d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; + d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; + d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; + d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; + d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; + d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; + d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; + d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; + d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; + d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; + d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; + d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; + // calculate determinant + det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w; + // calculate matrix inverse + det = 1/det; + for (int j = 0; j < 4; j++) + d[j] *= det; + return d; + } + + public static float4x4 MatrixRigidInverse(float4x4 m) + { + float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); + float4x4 rot = new float4x4(m); + rot.w = new float4(0f, 0f, 0f, 1f); + return trans_inverse * MatrixTranspose(rot); + } + public static float4x4 MatrixTranspose(float4x4 m) + { + return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w); + } + public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf) + { + float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height + float w = h / aspect; // view space width + return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0); + } + public static float4x4 MatrixTranslation(float3 t) + { + return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1); + } + public static float4x4 MatrixRotationZ(float angle_radians) + { + float s = (float)Math.Sin(angle_radians); + float c = (float)Math.Cos(angle_radians); + return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); + } + public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up) + { + float4x4 m = new float4x4(); + m.w.w = 1.0f; + m.w.setxyz(eye); + m.z.setxyz(float3.normalize(eye - at)); + m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz()))); + m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz())); + return MatrixRigidInverse(m); + } + + public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v) + { + // builds a 4x4 transformation matrix based on orientation q and translation v + float qx2 = q.x * q.x; + float qy2 = q.y * q.y; + float qz2 = q.z * q.z; + + float qxqy = q.x * q.y; + float qxqz = q.x * q.z; + float qxqw = q.x * q.w; + float qyqz = q.y * q.z; + float qyqw = q.y * q.w; + float qzqw = q.z * q.w; + + return new float4x4( + 1 - 2 * (qy2 + qz2), + 2 * (qxqy + qzqw), + 2 * (qxqz - qyqw), + 0, + 2 * (qxqy - qzqw), + 1 - 2 * (qx2 + qz2), + 2 * (qyqz + qxqw), + 0, + 2 * (qxqz + qyqw), + 2 * (qyqz - qxqw), + 1 - 2 * (qx2 + qy2), + 0, + v.x, + v.y, + v.z, + 1.0f); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs new file mode 100644 index 0000000000..9c5760d7e9 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs @@ -0,0 +1,128 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class int3 + { + public int x; + public int y; + public int z; + + public int3() + { + } + + public int3(int _x, int _y, int _z) + { + x = _x; + y = _y; + z = _z; + } + + public int this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + } + throw new ArgumentOutOfRangeException(); + } + set + { + switch (i) + { + case 0: x = value; return; + case 1: y = value; return; + case 2: z = value; return; + } + throw new ArgumentOutOfRangeException(); + } + } + + public override int GetHashCode() + { + return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); + } + + public override bool Equals(object obj) + { + int3 i = obj as int3; + if (i == null) + return false; + + return this == i; + } + + public static bool operator ==(int3 a, int3 b) + { + // If both are null, or both are same instance, return true. + if (System.Object.ReferenceEquals(a, b)) + return true; + // If one is null, but not both, return false. + if (((object)a == null) || ((object)b == null)) + return false; + + for (int i = 0; i < 3; i++) + { + if (a[i] != b[i]) + return false; + } + return true; + } + + public static bool operator !=(int3 a, int3 b) + { + return !(a == b); + } + + public static int3 roll3(int3 a) + { + int tmp = a[0]; + a[0] = a[1]; + a[1] = a[2]; + a[2] = tmp; + return a; + } + + public static bool isa(int3 a, int3 b) + { + return (a == b || roll3(a) == b || a == roll3(b)); + } + + public static bool b2b(int3 a, int3 b) + { + return isa(a, new int3(b[2], b[1], b[0])); + } + } +} diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs new file mode 100644 index 0000000000..c2b32e5a4d --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs @@ -0,0 +1,66 @@ +/* The MIT License + * + * Copyright (c) 2010 Intel Corporation. + * All rights reserved. + * + * Based on the convexdecomposition library from + * by John W. Ratcliff and Stan Melax. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +using System; + +namespace OpenSim.Region.Physics.ConvexDecompositionDotNet +{ + public class int4 + { + public int x; + public int y; + public int z; + public int w; + + public int4() + { + } + + public int4(int _x, int _y, int _z, int _w) + { + x = _x; + y = _y; + z = _z; + w = _w; + } + + public int this[int i] + { + get + { + switch (i) + { + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + throw new ArgumentOutOfRangeException(); + } + } + } +} diff --git a/ThirdPartyLicenses/ConvexDecompositionDotNet.txt b/ThirdPartyLicenses/ConvexDecompositionDotNet.txt new file mode 100644 index 0000000000..714ae898d8 --- /dev/null +++ b/ThirdPartyLicenses/ConvexDecompositionDotNet.txt @@ -0,0 +1,28 @@ +ConvexDecompositionDotNet +------------------------- + +The MIT License + +Copyright (c) 2010 Intel Corporation. +All rights reserved. + +Based on the convexdecomposition library from + by John W. Ratcliff and Stan Melax. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/prebuild.xml b/prebuild.xml index ed79b40c39..51dc70fd16 100644 --- a/prebuild.xml +++ b/prebuild.xml @@ -601,6 +601,64 @@ + + + + ../../../../bin/Physics/ + + + + + ../../../../bin/Physics/ + + + + ../../../../bin/ + + + + + + + + + + + + + + + + + + + + + + + + ../../../../bin/Physics/ + + + + + ../../../../bin/Physics/ + + + + ../../../../bin/ + + + + + + + + + + + +