From cf1e8b17237a7e4e5367e7bde887f97eba2547d8 Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Sun, 5 Apr 2009 08:43:29 +0000 Subject: * Fixing SVN properties --- .../BulletDotNETPlugin/BulletDotNETCharacter.cs | 2208 ++++++++++---------- 1 file changed, 1104 insertions(+), 1104 deletions(-) (limited to 'OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs') diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs index 6791671..9f3349e 100644 --- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs +++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs @@ -1,1104 +1,1104 @@ -using System; -using System.Reflection; -using BulletDotNET; -using OpenMetaverse; -using OpenSim.Framework; -using OpenSim.Region.Physics.Manager; -using log4net; - -namespace OpenSim.Region.Physics.BulletDotNETPlugin -{ - public class BulletDotNETCharacter : PhysicsActor - { - private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); - - public btRigidBody Body; - public btCollisionShape Shell; - public btVector3 tempVector1; - public btVector3 tempVector2; - public btVector3 tempVector3; - public btVector3 tempVector4; - - public btVector3 tempVector5RayCast; - public btVector3 tempVector6RayCast; - public btVector3 tempVector7RayCast; - - public btQuaternion tempQuat1; - public btTransform tempTrans1; - - public ClosestNotMeRayResultCallback ClosestCastResult; - private btTransform m_bodyTransform; - private btVector3 m_bodyPosition; - private btVector3 m_CapsuleOrientationAxis; - private btQuaternion m_bodyOrientation; - private btDefaultMotionState m_bodyMotionState; - private btGeneric6DofConstraint m_aMotor; - private PhysicsVector m_movementComparision; - private PhysicsVector m_position; - private PhysicsVector m_zeroPosition; - private bool m_zeroFlag = false; - private bool m_lastUpdateSent = false; - private PhysicsVector m_velocity; - private PhysicsVector m_target_velocity; - private PhysicsVector m_acceleration; - private PhysicsVector m_rotationalVelocity; - private bool m_pidControllerActive = true; - public float PID_D = 80.0f; - public float PID_P = 90.0f; - public float CAPSULE_RADIUS = 0.37f; - public float CAPSULE_LENGTH = 2.140599f; - public float heightFudgeFactor = 0.52f; - public float walkDivisor = 1.3f; - public float runDivisor = 0.8f; - private float m_mass = 80f; - public float m_density = 60f; - private bool m_flying = false; - private bool m_iscolliding = false; - private bool m_iscollidingGround = false; - private bool m_wascolliding = false; - private bool m_wascollidingGround = false; - private bool m_iscollidingObj = false; - private bool m_alwaysRun = false; - private bool m_hackSentFall = false; - private bool m_hackSentFly = false; - public uint m_localID = 0; - public bool m_returnCollisions = false; - // taints and their non-tainted counterparts - public bool m_isPhysical = false; // the current physical status - public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) - private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. - private bool m_taintRemove = false; - private bool m_taintedPosition = false; - private PhysicsVector m_taintedPosition_value; - private PhysicsVector m_taintedForce; - - private float m_buoyancy = 0f; - - // private CollisionLocker ode; - - private string m_name = String.Empty; - - private bool[] m_colliderarr = new bool[11]; - private bool[] m_colliderGroundarr = new bool[11]; - - - - private BulletDotNETScene m_parent_scene; - - public int m_eventsubscription = 0; - private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); - - public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) - { - m_taintedForce = new PhysicsVector(); - m_velocity = new PhysicsVector(); - m_target_velocity = new PhysicsVector(); - m_position = pos; - m_zeroPosition = new PhysicsVector(pos.X, pos.Y, pos.Z); // this is a class, not a struct. Must make new, or m_zeroPosition will == position regardless - m_acceleration = new PhysicsVector(); - m_parent_scene = parent_scene; - PID_D = pid_d; - PID_P = pid_p; - CAPSULE_RADIUS = capsule_radius; - m_density = density; - heightFudgeFactor = height_fudge_factor; - walkDivisor = walk_divisor; - runDivisor = rundivisor; - - for (int i = 0; i < 11; i++) - { - m_colliderarr[i] = false; - } - for (int i = 0; i < 11; i++) - { - m_colliderGroundarr[i] = false; - } - CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; - m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; - m_isPhysical = false; // current status: no ODE information exists - m_tainted_isPhysical = true; // new tainted status: need to create ODE information - - m_parent_scene.AddPhysicsActorTaint(this); - - m_name = avName; - tempVector1 = new btVector3(0, 0, 0); - tempVector2 = new btVector3(0, 0, 0); - tempVector3 = new btVector3(0, 0, 0); - tempVector4 = new btVector3(0, 0, 0); - - tempVector5RayCast = new btVector3(0, 0, 0); - tempVector6RayCast = new btVector3(0, 0, 0); - tempVector7RayCast = new btVector3(0, 0, 0); - - tempQuat1 = new btQuaternion(0, 0, 0, 1); - tempTrans1 = new btTransform(tempQuat1, tempVector1); - m_movementComparision = new PhysicsVector(0, 0, 0); - m_CapsuleOrientationAxis = new btVector3(1, 0, 1); - - - - } - - /// - /// This creates the Avatar's physical Surrogate at the position supplied - /// - /// - /// - /// - - // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access - // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only - // place that is safe to call this routine AvatarGeomAndBodyCreation. - private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) - { - - if (CAPSULE_LENGTH <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_LENGTH = 0.01f; - - } - - if (CAPSULE_RADIUS <= 0) - { - m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); - CAPSULE_RADIUS = 0.01f; - - } - - Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH); - - if (m_bodyPosition == null) - m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ); - - m_bodyPosition.setValue(npositionX, npositionY, npositionZ); - - if (m_bodyOrientation == null) - m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90)); - - if (m_bodyTransform == null) - m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); - else - { - m_bodyTransform.Dispose(); - m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); - } - - if (m_bodyMotionState == null) - m_bodyMotionState = new btDefaultMotionState(m_bodyTransform); - else - m_bodyMotionState.setWorldTransform(m_bodyTransform); - - m_mass = Mass; - - Body = new btRigidBody(m_mass, m_bodyMotionState, Shell); - Body.setUserPointer(new IntPtr((int)Body.Handle)); - - if (ClosestCastResult != null) - ClosestCastResult.Dispose(); - ClosestCastResult = new ClosestNotMeRayResultCallback(Body); - - m_parent_scene.AddRigidBody(Body); - Body.setActivationState(4); - if (m_aMotor != null) - { - if (m_aMotor.Handle != IntPtr.Zero) - { - m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); - m_aMotor.Dispose(); - } - m_aMotor = null; - } - - m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody, - m_parent_scene.TransZero, - m_parent_scene.TransZero, false); - m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero); - m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero); - - - } - public void Remove() - { - m_taintRemove = true; - } - public override bool Stopped - { - get { return m_zeroFlag; } - } - - public override PhysicsVector Size - { - get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } - set - { - m_pidControllerActive = true; - - PhysicsVector SetSize = value; - m_tainted_CAPSULE_LENGTH = (SetSize.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; - //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); - - Velocity = new PhysicsVector(0f, 0f, 0f); - - m_parent_scene.AddPhysicsActorTaint(this); - } - } - - /// - /// turn the PID controller on or off. - /// The PID Controller will turn on all by itself in many situations - /// - /// - public void SetPidStatus(bool status) - { - m_pidControllerActive = status; - } - - public override PrimitiveBaseShape Shape - { - set { return; } - } - - public override uint LocalID - { - set { m_localID = value; } - } - - public override bool Grabbed - { - set { return; } - } - - public override bool Selected - { - set { return; } - } - - - public override void CrossingFailure() - { - - } - - public override void link(PhysicsActor obj) - { - - } - - public override void delink() - { - - } - - public override void LockAngularMotion(PhysicsVector axis) - { - - } - - public override PhysicsVector Position - { - get { return m_position; } - set - { - m_taintedPosition_value = value; - m_position = value; - m_taintedPosition = true; - } - } - - public override float Mass - { - get - { - float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH); - return m_density * AVvolume; - } - } - - public override PhysicsVector Force - { - get { return new PhysicsVector(m_target_velocity.X, m_target_velocity.Y, m_target_velocity.Z); } - set { return; } - } - - public override int VehicleType - { - get { return 0; } - set { return; } - } - - public override void VehicleFloatParam(int param, float value) - { - - } - - public override void VehicleVectorParam(int param, PhysicsVector value) - { - - } - - public override void VehicleRotationParam(int param, Quaternion rotation) - { - - } - - public override void SetVolumeDetect(int param) - { - - } - - public override PhysicsVector GeometricCenter - { - get { return PhysicsVector.Zero; } - } - - public override PhysicsVector CenterOfMass - { - get { return PhysicsVector.Zero; } - } - - public override PhysicsVector Velocity - { - get - { - // There's a problem with PhysicsVector.Zero! Don't Use it Here! - if (m_zeroFlag) - return new PhysicsVector(0f, 0f, 0f); - m_lastUpdateSent = false; - return m_velocity; - } - set - { - m_pidControllerActive = true; - m_target_velocity = value; - } - } - - public override PhysicsVector Torque - { - get { return PhysicsVector.Zero; } - set { return; } - } - - public override float CollisionScore - { - get { return 0f; } - set { } - } - - public override PhysicsVector Acceleration - { - get { return m_acceleration; } - } - - public override Quaternion Orientation - { - get { return Quaternion.Identity; } - set - { - - } - } - - public override int PhysicsActorType - { - get { return (int)ActorTypes.Agent; } - set { return; } - } - - public override bool IsPhysical - { - get { return false; } - set { return; } - } - - public override bool Flying - { - get { return m_flying; } - set { m_flying = value; } - } - - public override bool SetAlwaysRun - { - get { return m_alwaysRun; } - set { m_alwaysRun = value; } - } - - - public override bool ThrottleUpdates - { - get { return false; } - set { return; } - } - - /// - /// Returns if the avatar is colliding in general. - /// This includes the ground and objects and avatar. - /// - public override bool IsColliding - { - get { return m_iscolliding; } - set - { - int i; - int truecount = 0; - int falsecount = 0; - - if (m_colliderarr.Length >= 10) - { - for (i = 0; i < 10; i++) - { - m_colliderarr[i] = m_colliderarr[i + 1]; - } - } - m_colliderarr[10] = value; - - for (i = 0; i < 11; i++) - { - if (m_colliderarr[i]) - { - truecount++; - } - else - { - falsecount++; - } - } - - // Equal truecounts and false counts means we're colliding with something. - m_log.DebugFormat("[PHYSICS]: TrueCount:{0}, FalseCount:{1}",truecount,falsecount); - if (falsecount > 1.2 * truecount) - { - m_iscolliding = false; - } - else - { - m_iscolliding = true; - } - if (m_wascolliding != m_iscolliding) - { - //base.SendCollisionUpdate(new CollisionEventUpdate()); - } - m_wascolliding = m_iscolliding; - } - } - - /// - /// Returns if an avatar is colliding with the ground - /// - public override bool CollidingGround - { - get { return m_iscollidingGround; } - set - { - // Collisions against the ground are not really reliable - // So, to get a consistant value we have to average the current result over time - // Currently we use 1 second = 10 calls to this. - int i; - int truecount = 0; - int falsecount = 0; - - if (m_colliderGroundarr.Length >= 10) - { - for (i = 0; i < 10; i++) - { - m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; - } - } - m_colliderGroundarr[10] = value; - - for (i = 0; i < 11; i++) - { - if (m_colliderGroundarr[i]) - { - truecount++; - } - else - { - falsecount++; - } - } - - // Equal truecounts and false counts means we're colliding with something. - - if (falsecount > 1.2 * truecount) - { - m_iscollidingGround = false; - } - else - { - m_iscollidingGround = true; - } - if (m_wascollidingGround != m_iscollidingGround) - { - //base.SendCollisionUpdate(new CollisionEventUpdate()); - } - m_wascollidingGround = m_iscollidingGround; - } - } - - /// - /// Returns if the avatar is colliding with an object - /// - public override bool CollidingObj - { - get { return m_iscollidingObj; } - set - { - m_iscollidingObj = value; - if (value) - m_pidControllerActive = false; - else - m_pidControllerActive = true; - } - } - - - public override bool FloatOnWater - { - set { return; } - } - - public override PhysicsVector RotationalVelocity - { - get { return m_rotationalVelocity; } - set { m_rotationalVelocity = value; } - } - - public override bool Kinematic - { - get { return false; } - set { } - } - - public override float Buoyancy - { - get { return m_buoyancy; } - set { m_buoyancy = value; } - } - - public override PhysicsVector PIDTarget { set { return; } } - public override bool PIDActive { set { return; } } - public override float PIDTau { set { return; } } - - public override bool PIDHoverActive - { - set { return; } - } - - public override float PIDHoverHeight - { - set { return; } - } - - public override PIDHoverType PIDHoverType - { - set { return; } - } - - public override float PIDHoverTau - { - set { return; } - } - - /// - /// Adds the force supplied to the Target Velocity - /// The PID controller takes this target velocity and tries to make it a reality - /// - /// - /// Is this a push by a script? - public override void AddForce(PhysicsVector force, bool pushforce) - { - if (pushforce) - { - m_pidControllerActive = false; - force *= 100f; - doForce(force, false); - //System.Console.WriteLine("Push!"); - //_target_velocity.X += force.X; - // _target_velocity.Y += force.Y; - //_target_velocity.Z += force.Z; - } - else - { - m_pidControllerActive = true; - m_target_velocity.X += force.X; - m_target_velocity.Y += force.Y; - m_target_velocity.Z += force.Z; - } - //m_lastUpdateSent = false; - } - - public void doForce(PhysicsVector force, bool now) - { - - tempVector3.setValue(force.X, force.Y, force.Z); - if (now) - { - Body.applyCentralForce(tempVector3); - } - else - { - m_taintedForce += force; - m_parent_scene.AddPhysicsActorTaint(this); - } - } - - public void doImpulse(PhysicsVector force, bool now) - { - - tempVector3.setValue(force.X, force.Y, force.Z); - if (now) - { - Body.applyCentralImpulse(tempVector3); - } - else - { - m_taintedForce += force; - m_parent_scene.AddPhysicsActorTaint(this); - } - } - - public override void AddAngularForce(PhysicsVector force, bool pushforce) - { - - } - - public override void SetMomentum(PhysicsVector momentum) - { - - } - - public override void SubscribeEvents(int ms) - { - m_eventsubscription = ms; - m_parent_scene.addCollisionEventReporting(this); - } - - public override void UnSubscribeEvents() - { - m_parent_scene.remCollisionEventReporting(this); - m_eventsubscription = 0; - } - - public override bool SubscribedEvents() - { - if (m_eventsubscription > 0) - return true; - return false; - } - - internal void Dispose() - { - if (Body.isInWorld()) - m_parent_scene.removeFromWorld(Body); - - if (m_aMotor.Handle != IntPtr.Zero) - m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); - - m_aMotor.Dispose(); m_aMotor = null; - ClosestCastResult.Dispose(); ClosestCastResult = null; - Body.Dispose(); Body = null; - Shell.Dispose(); Shell = null; - tempQuat1.Dispose(); - tempTrans1.Dispose(); - tempVector1.Dispose(); - tempVector2.Dispose(); - tempVector3.Dispose(); - tempVector4.Dispose(); - tempVector5RayCast.Dispose(); - tempVector6RayCast.Dispose(); - - } - - public void ProcessTaints(float timestep) - { - - if (m_tainted_isPhysical != m_isPhysical) - { - if (m_tainted_isPhysical) - { - // Create avatar capsule and related ODE data - if (!(Shell == null && Body == null)) - { - m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " - + (Shell != null ? "Shell " : "") - + (Body != null ? "Body " : "")); - } - AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z); - - - } - else - { - // destroy avatar capsule and related ODE data - - Dispose(); - tempVector1 = new btVector3(0, 0, 0); - tempVector2 = new btVector3(0, 0, 0); - tempVector3 = new btVector3(0, 0, 0); - tempVector4 = new btVector3(0, 0, 0); - - tempVector5RayCast = new btVector3(0, 0, 0); - tempVector6RayCast = new btVector3(0, 0, 0); - tempVector7RayCast = new btVector3(0, 0, 0); - - tempQuat1 = new btQuaternion(0, 0, 0, 1); - tempTrans1 = new btTransform(tempQuat1, tempVector1); - m_movementComparision = new PhysicsVector(0, 0, 0); - m_CapsuleOrientationAxis = new btVector3(1, 0, 1); - } - - m_isPhysical = m_tainted_isPhysical; - } - - if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) - { - if (Body != null) - { - - m_pidControllerActive = true; - // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() - //d.JointDestroy(Amotor); - float prevCapsule = CAPSULE_LENGTH; - CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; - //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); - Dispose(); - - tempVector1 = new btVector3(0, 0, 0); - tempVector2 = new btVector3(0, 0, 0); - tempVector3 = new btVector3(0, 0, 0); - tempVector4 = new btVector3(0, 0, 0); - - tempVector5RayCast = new btVector3(0, 0, 0); - tempVector6RayCast = new btVector3(0, 0, 0); - tempVector7RayCast = new btVector3(0, 0, 0); - - tempQuat1 = new btQuaternion(0, 0, 0, 1); - tempTrans1 = new btTransform(tempQuat1, tempVector1); - m_movementComparision = new PhysicsVector(0, 0, 0); - m_CapsuleOrientationAxis = new btVector3(1, 0, 1); - - AvatarGeomAndBodyCreation(m_position.X, m_position.Y, - m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); - Velocity = new PhysicsVector(0f, 0f, 0f); - - } - else - { - m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " - + (Shell == null ? "Shell " : "") - + (Body == null ? "Body " : "")); - } - } - if (m_taintRemove) - { - Dispose(); - } - } - - /// - /// Called from Simulate - /// This is the avatar's movement control + PID Controller - /// - /// - public void Move(float timeStep) - { - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - if (Body == null) - return; - tempTrans1.Dispose(); - tempTrans1 = Body.getInterpolationWorldTransform(); - tempVector1.Dispose(); - tempVector1 = tempTrans1.getOrigin(); - tempVector2.Dispose(); - tempVector2 = Body.getInterpolationLinearVelocity(); - - if (m_pidControllerActive == false) - { - m_zeroPosition.X = tempVector1.getX(); - m_zeroPosition.Y = tempVector1.getY(); - m_zeroPosition.Z = tempVector1.getZ(); - } - //PidStatus = true; - - PhysicsVector vec = new PhysicsVector(); - - PhysicsVector vel = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); - - float movementdivisor = 1f; - - if (!m_alwaysRun) - { - movementdivisor = walkDivisor; - } - else - { - movementdivisor = runDivisor; - } - - // if velocity is zero, use position control; otherwise, velocity control - if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding) - { - // keep track of where we stopped. No more slippin' & slidin' - if (!m_zeroFlag) - { - m_zeroFlag = true; - m_zeroPosition.X = tempVector1.getX(); - m_zeroPosition.Y = tempVector1.getY(); - m_zeroPosition.Z = tempVector1.getZ(); - } - if (m_pidControllerActive) - { - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); - vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2); - vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2); - if (m_flying) - { - vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P; - } - } - //PidStatus = true; - } - else - { - m_pidControllerActive = true; - m_zeroFlag = false; - if (m_iscolliding && !m_flying) - { - // We're standing on something - vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D); - vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); - } - else if (m_iscolliding && m_flying) - { - // We're flying and colliding with something - vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16); - vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16); - } - else if (!m_iscolliding && m_flying) - { - // we're in mid air suspended - vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6); - vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6); - } - - if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f) - { - // We're colliding with something and we're not flying but we're moving - // This means we're walking or running. - PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); - vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P; - if (m_target_velocity.X > 0) - { - vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; - } - if (m_target_velocity.Y > 0) - { - vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; - } - } - else if (!m_iscolliding && !m_flying) - { - // we're not colliding and we're not flying so that means we're falling! - // m_iscolliding includes collisions with the ground. - - // d.Vector3 pos = d.BodyGetPosition(Body); - if (m_target_velocity.X > 0) - { - vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; - } - if (m_target_velocity.Y > 0) - { - vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; - } - } - - - if (m_flying) - { - vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D); - } - } - if (m_flying) - { - //vec.Z += ((-1 * m_parent_scene.gravityz) * m_mass); - } - if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f)))) - { - Body.setFriction(0.001f); - //m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString()); - } - - if (Body != null) - { - int activationstate = Body.getActivationState(); - if (activationstate == 0) - { - Body.forceActivationState(1); - } - - - } - doImpulse(vec, true); - } - - /// - /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. - /// - public void UpdatePositionAndVelocity() - { - if (Body == null) - return; - //int val = Environment.TickCount; - CheckIfStandingOnObject(); - //m_log.DebugFormat("time:{0}", Environment.TickCount - val); - - //IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody); - - tempTrans1.Dispose(); - tempTrans1 = Body.getInterpolationWorldTransform(); - tempVector1.Dispose(); - tempVector1 = tempTrans1.getOrigin(); - tempVector2.Dispose(); - tempVector2 = Body.getInterpolationLinearVelocity(); - - // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! - PhysicsVector vec = new PhysicsVector(tempVector1.getX(),tempVector1.getY(),tempVector1.getZ()); - - // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) - if (vec.X < 0.0f) vec.X = 0.0f; - if (vec.Y < 0.0f) vec.Y = 0.0f; - if (vec.X > (int)Constants.RegionSize - 0.2f) vec.X = (int)Constants.RegionSize - 0.2f; - if (vec.Y > (int)Constants.RegionSize - 0.2f) vec.Y = (int)Constants.RegionSize - 0.2f; - - m_position.X = vec.X; - m_position.Y = vec.Y; - m_position.Z = vec.Z; - - // Did we move last? = zeroflag - // This helps keep us from sliding all over - - if (m_zeroFlag) - { - m_velocity.X = 0.0f; - m_velocity.Y = 0.0f; - m_velocity.Z = 0.0f; - - // Did we send out the 'stopped' message? - if (!m_lastUpdateSent) - { - m_lastUpdateSent = true; - //base.RequestPhysicsterseUpdate(); - - } - } - else - { - m_lastUpdateSent = false; - vec = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); - m_velocity.X = (vec.X); - m_velocity.Y = (vec.Y); - - m_velocity.Z = (vec.Z); - - if (m_velocity.Z < -6 && !m_hackSentFall) - { - m_hackSentFall = true; - m_pidControllerActive = false; - } - else if (m_flying && !m_hackSentFly) - { - //m_hackSentFly = true; - //base.SendCollisionUpdate(new CollisionEventUpdate()); - } - else - { - m_hackSentFly = false; - m_hackSentFall = false; - } - } - if (Body != null) - { - if (Body.getFriction() < 0.9f) - Body.setFriction(0.9f); - } - //if (Body != null) - // Body.clearForces(); - } - - public void CheckIfStandingOnObject() - { - - float capsuleHalfHeight = ((CAPSULE_LENGTH + 2*CAPSULE_RADIUS)*0.5f); - - tempVector5RayCast.setValue(m_position.X, m_position.Y, m_position.Z); - tempVector6RayCast.setValue(m_position.X, m_position.Y, m_position.Z - 1 * capsuleHalfHeight * 1.1f); - - - ClosestCastResult.Dispose(); - ClosestCastResult = new ClosestNotMeRayResultCallback(Body); - - try - { - m_parent_scene.getBulletWorld().rayTest(tempVector5RayCast, tempVector6RayCast, ClosestCastResult); - } - catch (AccessViolationException) - { - m_log.Debug("BAD!"); - } - if (ClosestCastResult.hasHit()) - { - - if (tempVector7RayCast != null) - tempVector7RayCast.Dispose(); - - //tempVector7RayCast = ClosestCastResult.getHitPointWorld(); - - /*if (tempVector7RayCast == null) // null == no result also - { - CollidingObj = false; - IsColliding = false; - CollidingGround = false; - - return; - } - float zVal = tempVector7RayCast.getZ(); - if (zVal != 0) - m_log.Debug("[PHYSICS]: HAAAA"); - if (zVal < m_position.Z && zVal > ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) *0.5f)) - { - CollidingObj = true; - IsColliding = true; - } - else - { - CollidingObj = false; - IsColliding = false; - CollidingGround = false; - }*/ - - //height+2*radius = capsule full length - //CollidingObj = true; - //IsColliding = true; - m_iscolliding = true; - } - else - { - //CollidingObj = false; - //IsColliding = false; - //CollidingGround = false; - m_iscolliding = false; - } - } - } - -} +using System; +using System.Reflection; +using BulletDotNET; +using OpenMetaverse; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; +using log4net; + +namespace OpenSim.Region.Physics.BulletDotNETPlugin +{ + public class BulletDotNETCharacter : PhysicsActor + { + private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); + + public btRigidBody Body; + public btCollisionShape Shell; + public btVector3 tempVector1; + public btVector3 tempVector2; + public btVector3 tempVector3; + public btVector3 tempVector4; + + public btVector3 tempVector5RayCast; + public btVector3 tempVector6RayCast; + public btVector3 tempVector7RayCast; + + public btQuaternion tempQuat1; + public btTransform tempTrans1; + + public ClosestNotMeRayResultCallback ClosestCastResult; + private btTransform m_bodyTransform; + private btVector3 m_bodyPosition; + private btVector3 m_CapsuleOrientationAxis; + private btQuaternion m_bodyOrientation; + private btDefaultMotionState m_bodyMotionState; + private btGeneric6DofConstraint m_aMotor; + private PhysicsVector m_movementComparision; + private PhysicsVector m_position; + private PhysicsVector m_zeroPosition; + private bool m_zeroFlag = false; + private bool m_lastUpdateSent = false; + private PhysicsVector m_velocity; + private PhysicsVector m_target_velocity; + private PhysicsVector m_acceleration; + private PhysicsVector m_rotationalVelocity; + private bool m_pidControllerActive = true; + public float PID_D = 80.0f; + public float PID_P = 90.0f; + public float CAPSULE_RADIUS = 0.37f; + public float CAPSULE_LENGTH = 2.140599f; + public float heightFudgeFactor = 0.52f; + public float walkDivisor = 1.3f; + public float runDivisor = 0.8f; + private float m_mass = 80f; + public float m_density = 60f; + private bool m_flying = false; + private bool m_iscolliding = false; + private bool m_iscollidingGround = false; + private bool m_wascolliding = false; + private bool m_wascollidingGround = false; + private bool m_iscollidingObj = false; + private bool m_alwaysRun = false; + private bool m_hackSentFall = false; + private bool m_hackSentFly = false; + public uint m_localID = 0; + public bool m_returnCollisions = false; + // taints and their non-tainted counterparts + public bool m_isPhysical = false; // the current physical status + public bool m_tainted_isPhysical = false; // set when the physical status is tainted (false=not existing in physics engine, true=existing) + private float m_tainted_CAPSULE_LENGTH; // set when the capsule length changes. + private bool m_taintRemove = false; + private bool m_taintedPosition = false; + private PhysicsVector m_taintedPosition_value; + private PhysicsVector m_taintedForce; + + private float m_buoyancy = 0f; + + // private CollisionLocker ode; + + private string m_name = String.Empty; + + private bool[] m_colliderarr = new bool[11]; + private bool[] m_colliderGroundarr = new bool[11]; + + + + private BulletDotNETScene m_parent_scene; + + public int m_eventsubscription = 0; + private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); + + public BulletDotNETCharacter(string avName, BulletDotNETScene parent_scene, PhysicsVector pos, PhysicsVector size, float pid_d, float pid_p, float capsule_radius, float tensor, float density, float height_fudge_factor, float walk_divisor, float rundivisor) + { + m_taintedForce = new PhysicsVector(); + m_velocity = new PhysicsVector(); + m_target_velocity = new PhysicsVector(); + m_position = pos; + m_zeroPosition = new PhysicsVector(pos.X, pos.Y, pos.Z); // this is a class, not a struct. Must make new, or m_zeroPosition will == position regardless + m_acceleration = new PhysicsVector(); + m_parent_scene = parent_scene; + PID_D = pid_d; + PID_P = pid_p; + CAPSULE_RADIUS = capsule_radius; + m_density = density; + heightFudgeFactor = height_fudge_factor; + walkDivisor = walk_divisor; + runDivisor = rundivisor; + + for (int i = 0; i < 11; i++) + { + m_colliderarr[i] = false; + } + for (int i = 0; i < 11; i++) + { + m_colliderGroundarr[i] = false; + } + CAPSULE_LENGTH = (size.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + m_tainted_CAPSULE_LENGTH = CAPSULE_LENGTH; + m_isPhysical = false; // current status: no ODE information exists + m_tainted_isPhysical = true; // new tainted status: need to create ODE information + + m_parent_scene.AddPhysicsActorTaint(this); + + m_name = avName; + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + + + + } + + /// + /// This creates the Avatar's physical Surrogate at the position supplied + /// + /// + /// + /// + + // WARNING: This MUST NOT be called outside of ProcessTaints, else we can have unsynchronized access + // to ODE internals. ProcessTaints is called from within thread-locked Simulate(), so it is the only + // place that is safe to call this routine AvatarGeomAndBodyCreation. + private void AvatarGeomAndBodyCreation(float npositionX, float npositionY, float npositionZ) + { + + if (CAPSULE_LENGTH <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_LENGTH = 0.01f; + + } + + if (CAPSULE_RADIUS <= 0) + { + m_log.Warn("[PHYSICS]: The capsule size you specified in opensim.ini is invalid! Setting it to the smallest possible size!"); + CAPSULE_RADIUS = 0.01f; + + } + + Shell = new btCapsuleShape(CAPSULE_RADIUS, CAPSULE_LENGTH); + + if (m_bodyPosition == null) + m_bodyPosition = new btVector3(npositionX, npositionY, npositionZ); + + m_bodyPosition.setValue(npositionX, npositionY, npositionZ); + + if (m_bodyOrientation == null) + m_bodyOrientation = new btQuaternion(m_CapsuleOrientationAxis, (Utils.DEG_TO_RAD * 90)); + + if (m_bodyTransform == null) + m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); + else + { + m_bodyTransform.Dispose(); + m_bodyTransform = new btTransform(m_bodyOrientation, m_bodyPosition); + } + + if (m_bodyMotionState == null) + m_bodyMotionState = new btDefaultMotionState(m_bodyTransform); + else + m_bodyMotionState.setWorldTransform(m_bodyTransform); + + m_mass = Mass; + + Body = new btRigidBody(m_mass, m_bodyMotionState, Shell); + Body.setUserPointer(new IntPtr((int)Body.Handle)); + + if (ClosestCastResult != null) + ClosestCastResult.Dispose(); + ClosestCastResult = new ClosestNotMeRayResultCallback(Body); + + m_parent_scene.AddRigidBody(Body); + Body.setActivationState(4); + if (m_aMotor != null) + { + if (m_aMotor.Handle != IntPtr.Zero) + { + m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); + m_aMotor.Dispose(); + } + m_aMotor = null; + } + + m_aMotor = new btGeneric6DofConstraint(Body, m_parent_scene.TerrainBody, + m_parent_scene.TransZero, + m_parent_scene.TransZero, false); + m_aMotor.setAngularLowerLimit(m_parent_scene.VectorZero); + m_aMotor.setAngularUpperLimit(m_parent_scene.VectorZero); + + + } + public void Remove() + { + m_taintRemove = true; + } + public override bool Stopped + { + get { return m_zeroFlag; } + } + + public override PhysicsVector Size + { + get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } + set + { + m_pidControllerActive = true; + + PhysicsVector SetSize = value; + m_tainted_CAPSULE_LENGTH = (SetSize.Z * 1.15f) - CAPSULE_RADIUS * 2.0f; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + + Velocity = new PhysicsVector(0f, 0f, 0f); + + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + /// + /// turn the PID controller on or off. + /// The PID Controller will turn on all by itself in many situations + /// + /// + public void SetPidStatus(bool status) + { + m_pidControllerActive = status; + } + + public override PrimitiveBaseShape Shape + { + set { return; } + } + + public override uint LocalID + { + set { m_localID = value; } + } + + public override bool Grabbed + { + set { return; } + } + + public override bool Selected + { + set { return; } + } + + + public override void CrossingFailure() + { + + } + + public override void link(PhysicsActor obj) + { + + } + + public override void delink() + { + + } + + public override void LockAngularMotion(PhysicsVector axis) + { + + } + + public override PhysicsVector Position + { + get { return m_position; } + set + { + m_taintedPosition_value = value; + m_position = value; + m_taintedPosition = true; + } + } + + public override float Mass + { + get + { + float AVvolume = (float)(Math.PI * Math.Pow(CAPSULE_RADIUS, 2) * CAPSULE_LENGTH); + return m_density * AVvolume; + } + } + + public override PhysicsVector Force + { + get { return new PhysicsVector(m_target_velocity.X, m_target_velocity.Y, m_target_velocity.Z); } + set { return; } + } + + public override int VehicleType + { + get { return 0; } + set { return; } + } + + public override void VehicleFloatParam(int param, float value) + { + + } + + public override void VehicleVectorParam(int param, PhysicsVector value) + { + + } + + public override void VehicleRotationParam(int param, Quaternion rotation) + { + + } + + public override void SetVolumeDetect(int param) + { + + } + + public override PhysicsVector GeometricCenter + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector CenterOfMass + { + get { return PhysicsVector.Zero; } + } + + public override PhysicsVector Velocity + { + get + { + // There's a problem with PhysicsVector.Zero! Don't Use it Here! + if (m_zeroFlag) + return new PhysicsVector(0f, 0f, 0f); + m_lastUpdateSent = false; + return m_velocity; + } + set + { + m_pidControllerActive = true; + m_target_velocity = value; + } + } + + public override PhysicsVector Torque + { + get { return PhysicsVector.Zero; } + set { return; } + } + + public override float CollisionScore + { + get { return 0f; } + set { } + } + + public override PhysicsVector Acceleration + { + get { return m_acceleration; } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set + { + + } + } + + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + + public override bool IsPhysical + { + get { return false; } + set { return; } + } + + public override bool Flying + { + get { return m_flying; } + set { m_flying = value; } + } + + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + + + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + + /// + /// Returns if the avatar is colliding in general. + /// This includes the ground and objects and avatar. + /// + public override bool IsColliding + { + get { return m_iscolliding; } + set + { + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderarr[i] = m_colliderarr[i + 1]; + } + } + m_colliderarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + m_log.DebugFormat("[PHYSICS]: TrueCount:{0}, FalseCount:{1}",truecount,falsecount); + if (falsecount > 1.2 * truecount) + { + m_iscolliding = false; + } + else + { + m_iscolliding = true; + } + if (m_wascolliding != m_iscolliding) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascolliding = m_iscolliding; + } + } + + /// + /// Returns if an avatar is colliding with the ground + /// + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + // Collisions against the ground are not really reliable + // So, to get a consistant value we have to average the current result over time + // Currently we use 1 second = 10 calls to this. + int i; + int truecount = 0; + int falsecount = 0; + + if (m_colliderGroundarr.Length >= 10) + { + for (i = 0; i < 10; i++) + { + m_colliderGroundarr[i] = m_colliderGroundarr[i + 1]; + } + } + m_colliderGroundarr[10] = value; + + for (i = 0; i < 11; i++) + { + if (m_colliderGroundarr[i]) + { + truecount++; + } + else + { + falsecount++; + } + } + + // Equal truecounts and false counts means we're colliding with something. + + if (falsecount > 1.2 * truecount) + { + m_iscollidingGround = false; + } + else + { + m_iscollidingGround = true; + } + if (m_wascollidingGround != m_iscollidingGround) + { + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + m_wascollidingGround = m_iscollidingGround; + } + } + + /// + /// Returns if the avatar is colliding with an object + /// + public override bool CollidingObj + { + get { return m_iscollidingObj; } + set + { + m_iscollidingObj = value; + if (value) + m_pidControllerActive = false; + else + m_pidControllerActive = true; + } + } + + + public override bool FloatOnWater + { + set { return; } + } + + public override PhysicsVector RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override float Buoyancy + { + get { return m_buoyancy; } + set { m_buoyancy = value; } + } + + public override PhysicsVector PIDTarget { set { return; } } + public override bool PIDActive { set { return; } } + public override float PIDTau { set { return; } } + + public override bool PIDHoverActive + { + set { return; } + } + + public override float PIDHoverHeight + { + set { return; } + } + + public override PIDHoverType PIDHoverType + { + set { return; } + } + + public override float PIDHoverTau + { + set { return; } + } + + /// + /// Adds the force supplied to the Target Velocity + /// The PID controller takes this target velocity and tries to make it a reality + /// + /// + /// Is this a push by a script? + public override void AddForce(PhysicsVector force, bool pushforce) + { + if (pushforce) + { + m_pidControllerActive = false; + force *= 100f; + doForce(force, false); + //System.Console.WriteLine("Push!"); + //_target_velocity.X += force.X; + // _target_velocity.Y += force.Y; + //_target_velocity.Z += force.Z; + } + else + { + m_pidControllerActive = true; + m_target_velocity.X += force.X; + m_target_velocity.Y += force.Y; + m_target_velocity.Z += force.Z; + } + //m_lastUpdateSent = false; + } + + public void doForce(PhysicsVector force, bool now) + { + + tempVector3.setValue(force.X, force.Y, force.Z); + if (now) + { + Body.applyCentralForce(tempVector3); + } + else + { + m_taintedForce += force; + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + public void doImpulse(PhysicsVector force, bool now) + { + + tempVector3.setValue(force.X, force.Y, force.Z); + if (now) + { + Body.applyCentralImpulse(tempVector3); + } + else + { + m_taintedForce += force; + m_parent_scene.AddPhysicsActorTaint(this); + } + } + + public override void AddAngularForce(PhysicsVector force, bool pushforce) + { + + } + + public override void SetMomentum(PhysicsVector momentum) + { + + } + + public override void SubscribeEvents(int ms) + { + m_eventsubscription = ms; + m_parent_scene.addCollisionEventReporting(this); + } + + public override void UnSubscribeEvents() + { + m_parent_scene.remCollisionEventReporting(this); + m_eventsubscription = 0; + } + + public override bool SubscribedEvents() + { + if (m_eventsubscription > 0) + return true; + return false; + } + + internal void Dispose() + { + if (Body.isInWorld()) + m_parent_scene.removeFromWorld(Body); + + if (m_aMotor.Handle != IntPtr.Zero) + m_parent_scene.getBulletWorld().removeConstraint(m_aMotor); + + m_aMotor.Dispose(); m_aMotor = null; + ClosestCastResult.Dispose(); ClosestCastResult = null; + Body.Dispose(); Body = null; + Shell.Dispose(); Shell = null; + tempQuat1.Dispose(); + tempTrans1.Dispose(); + tempVector1.Dispose(); + tempVector2.Dispose(); + tempVector3.Dispose(); + tempVector4.Dispose(); + tempVector5RayCast.Dispose(); + tempVector6RayCast.Dispose(); + + } + + public void ProcessTaints(float timestep) + { + + if (m_tainted_isPhysical != m_isPhysical) + { + if (m_tainted_isPhysical) + { + // Create avatar capsule and related ODE data + if (!(Shell == null && Body == null)) + { + m_log.Warn("[PHYSICS]: re-creating the following avatar ODE data, even though it already exists - " + + (Shell != null ? "Shell " : "") + + (Body != null ? "Body " : "")); + } + AvatarGeomAndBodyCreation(m_position.X, m_position.Y, m_position.Z); + + + } + else + { + // destroy avatar capsule and related ODE data + + Dispose(); + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + } + + m_isPhysical = m_tainted_isPhysical; + } + + if (m_tainted_CAPSULE_LENGTH != CAPSULE_LENGTH) + { + if (Body != null) + { + + m_pidControllerActive = true; + // no lock needed on _parent_scene.OdeLock because we are called from within the thread lock in OdePlugin's simulate() + //d.JointDestroy(Amotor); + float prevCapsule = CAPSULE_LENGTH; + CAPSULE_LENGTH = m_tainted_CAPSULE_LENGTH; + //m_log.Info("[SIZE]: " + CAPSULE_LENGTH.ToString()); + Dispose(); + + tempVector1 = new btVector3(0, 0, 0); + tempVector2 = new btVector3(0, 0, 0); + tempVector3 = new btVector3(0, 0, 0); + tempVector4 = new btVector3(0, 0, 0); + + tempVector5RayCast = new btVector3(0, 0, 0); + tempVector6RayCast = new btVector3(0, 0, 0); + tempVector7RayCast = new btVector3(0, 0, 0); + + tempQuat1 = new btQuaternion(0, 0, 0, 1); + tempTrans1 = new btTransform(tempQuat1, tempVector1); + m_movementComparision = new PhysicsVector(0, 0, 0); + m_CapsuleOrientationAxis = new btVector3(1, 0, 1); + + AvatarGeomAndBodyCreation(m_position.X, m_position.Y, + m_position.Z + (Math.Abs(CAPSULE_LENGTH - prevCapsule) * 2)); + Velocity = new PhysicsVector(0f, 0f, 0f); + + } + else + { + m_log.Warn("[PHYSICS]: trying to change capsule size, but the following ODE data is missing - " + + (Shell == null ? "Shell " : "") + + (Body == null ? "Body " : "")); + } + } + if (m_taintRemove) + { + Dispose(); + } + } + + /// + /// Called from Simulate + /// This is the avatar's movement control + PID Controller + /// + /// + public void Move(float timeStep) + { + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + if (Body == null) + return; + tempTrans1.Dispose(); + tempTrans1 = Body.getInterpolationWorldTransform(); + tempVector1.Dispose(); + tempVector1 = tempTrans1.getOrigin(); + tempVector2.Dispose(); + tempVector2 = Body.getInterpolationLinearVelocity(); + + if (m_pidControllerActive == false) + { + m_zeroPosition.X = tempVector1.getX(); + m_zeroPosition.Y = tempVector1.getY(); + m_zeroPosition.Z = tempVector1.getZ(); + } + //PidStatus = true; + + PhysicsVector vec = new PhysicsVector(); + + PhysicsVector vel = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); + + float movementdivisor = 1f; + + if (!m_alwaysRun) + { + movementdivisor = walkDivisor; + } + else + { + movementdivisor = runDivisor; + } + + // if velocity is zero, use position control; otherwise, velocity control + if (m_target_velocity.X == 0.0f && m_target_velocity.Y == 0.0f && m_target_velocity.Z == 0.0f && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!m_zeroFlag) + { + m_zeroFlag = true; + m_zeroPosition.X = tempVector1.getX(); + m_zeroPosition.Y = tempVector1.getY(); + m_zeroPosition.Z = tempVector1.getZ(); + } + if (m_pidControllerActive) + { + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); + vec.X = (m_target_velocity.X - vel.X) * (PID_D) + (m_zeroPosition.X - pos.X) * (PID_P * 2); + vec.Y = (m_target_velocity.Y - vel.Y) * (PID_D) + (m_zeroPosition.Y - pos.Y) * (PID_P * 2); + if (m_flying) + { + vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D) + (m_zeroPosition.Z - pos.Z) * PID_P; + } + } + //PidStatus = true; + } + else + { + m_pidControllerActive = true; + m_zeroFlag = false; + if (m_iscolliding && !m_flying) + { + // We're standing on something + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D); + } + else if (m_iscolliding && m_flying) + { + // We're flying and colliding with something + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 16); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 16); + } + else if (!m_iscolliding && m_flying) + { + // we're in mid air suspended + vec.X = ((m_target_velocity.X / movementdivisor) - vel.X) * (PID_D / 6); + vec.Y = ((m_target_velocity.Y / movementdivisor) - vel.Y) * (PID_D / 6); + } + + if (m_iscolliding && !m_flying && m_target_velocity.Z > 0.0f) + { + // We're colliding with something and we're not flying but we're moving + // This means we're walking or running. + PhysicsVector pos = new PhysicsVector(tempVector1.getX(), tempVector1.getY(), tempVector1.getZ()); + vec.Z = (m_target_velocity.Z - vel.Z) * PID_D + (m_zeroPosition.Z - pos.Z) * PID_P; + if (m_target_velocity.X > 0) + { + vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (m_target_velocity.Y > 0) + { + vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + else if (!m_iscolliding && !m_flying) + { + // we're not colliding and we're not flying so that means we're falling! + // m_iscolliding includes collisions with the ground. + + // d.Vector3 pos = d.BodyGetPosition(Body); + if (m_target_velocity.X > 0) + { + vec.X = ((m_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (m_target_velocity.Y > 0) + { + vec.Y = ((m_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + + + if (m_flying) + { + vec.Z = (m_target_velocity.Z - vel.Z) * (PID_D); + } + } + if (m_flying) + { + //vec.Z += ((-1 * m_parent_scene.gravityz) * m_mass); + } + if (Body != null && (((m_target_velocity.X > 0.2f || m_target_velocity.X < -0.2f) || (m_target_velocity.Y > 0.2f || m_target_velocity.Y < -0.2f)))) + { + Body.setFriction(0.001f); + //m_log.DebugFormat("[PHYSICS]: Avatar force applied: {0}, Target:{1}", vec.ToString(), m_target_velocity.ToString()); + } + + if (Body != null) + { + int activationstate = Body.getActivationState(); + if (activationstate == 0) + { + Body.forceActivationState(1); + } + + + } + doImpulse(vec, true); + } + + /// + /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. + /// + public void UpdatePositionAndVelocity() + { + if (Body == null) + return; + //int val = Environment.TickCount; + CheckIfStandingOnObject(); + //m_log.DebugFormat("time:{0}", Environment.TickCount - val); + + //IsColliding = Body.checkCollideWith(m_parent_scene.TerrainBody); + + tempTrans1.Dispose(); + tempTrans1 = Body.getInterpolationWorldTransform(); + tempVector1.Dispose(); + tempVector1 = tempTrans1.getOrigin(); + tempVector2.Dispose(); + tempVector2 = Body.getInterpolationLinearVelocity(); + + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + PhysicsVector vec = new PhysicsVector(tempVector1.getX(),tempVector1.getY(),tempVector1.getZ()); + + // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) + if (vec.X < 0.0f) vec.X = 0.0f; + if (vec.Y < 0.0f) vec.Y = 0.0f; + if (vec.X > (int)Constants.RegionSize - 0.2f) vec.X = (int)Constants.RegionSize - 0.2f; + if (vec.Y > (int)Constants.RegionSize - 0.2f) vec.Y = (int)Constants.RegionSize - 0.2f; + + m_position.X = vec.X; + m_position.Y = vec.Y; + m_position.Z = vec.Z; + + // Did we move last? = zeroflag + // This helps keep us from sliding all over + + if (m_zeroFlag) + { + m_velocity.X = 0.0f; + m_velocity.Y = 0.0f; + m_velocity.Z = 0.0f; + + // Did we send out the 'stopped' message? + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + //base.RequestPhysicsterseUpdate(); + + } + } + else + { + m_lastUpdateSent = false; + vec = new PhysicsVector(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); + m_velocity.X = (vec.X); + m_velocity.Y = (vec.Y); + + m_velocity.Z = (vec.Z); + + if (m_velocity.Z < -6 && !m_hackSentFall) + { + m_hackSentFall = true; + m_pidControllerActive = false; + } + else if (m_flying && !m_hackSentFly) + { + //m_hackSentFly = true; + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else + { + m_hackSentFly = false; + m_hackSentFall = false; + } + } + if (Body != null) + { + if (Body.getFriction() < 0.9f) + Body.setFriction(0.9f); + } + //if (Body != null) + // Body.clearForces(); + } + + public void CheckIfStandingOnObject() + { + + float capsuleHalfHeight = ((CAPSULE_LENGTH + 2*CAPSULE_RADIUS)*0.5f); + + tempVector5RayCast.setValue(m_position.X, m_position.Y, m_position.Z); + tempVector6RayCast.setValue(m_position.X, m_position.Y, m_position.Z - 1 * capsuleHalfHeight * 1.1f); + + + ClosestCastResult.Dispose(); + ClosestCastResult = new ClosestNotMeRayResultCallback(Body); + + try + { + m_parent_scene.getBulletWorld().rayTest(tempVector5RayCast, tempVector6RayCast, ClosestCastResult); + } + catch (AccessViolationException) + { + m_log.Debug("BAD!"); + } + if (ClosestCastResult.hasHit()) + { + + if (tempVector7RayCast != null) + tempVector7RayCast.Dispose(); + + //tempVector7RayCast = ClosestCastResult.getHitPointWorld(); + + /*if (tempVector7RayCast == null) // null == no result also + { + CollidingObj = false; + IsColliding = false; + CollidingGround = false; + + return; + } + float zVal = tempVector7RayCast.getZ(); + if (zVal != 0) + m_log.Debug("[PHYSICS]: HAAAA"); + if (zVal < m_position.Z && zVal > ((CAPSULE_LENGTH + 2 * CAPSULE_RADIUS) *0.5f)) + { + CollidingObj = true; + IsColliding = true; + } + else + { + CollidingObj = false; + IsColliding = false; + CollidingGround = false; + }*/ + + //height+2*radius = capsule full length + //CollidingObj = true; + //IsColliding = true; + m_iscolliding = true; + } + else + { + //CollidingObj = false; + //IsColliding = false; + //CollidingGround = false; + m_iscolliding = false; + } + } + } + +} -- cgit v1.1