From 4f05347246c2aedc8adb43772e6dc9fb92ef6c9d Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Tue, 4 Dec 2007 02:51:09 +0000 Subject: * Split out the ODEPlugin Nested classes. --- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 477 +++++++++++++++++++++++ 1 file changed, 477 insertions(+) create mode 100644 OpenSim/Region/Physics/OdePlugin/ODECharacter.cs (limited to 'OpenSim/Region/Physics/OdePlugin/ODECharacter.cs') diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs new file mode 100644 index 0000000..cda6af2 --- /dev/null +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -0,0 +1,477 @@ +using System; +using System.Collections.Generic; +using Axiom.Math; +using Ode.NET; +using OpenSim.Framework; +using OpenSim.Region.Physics.Manager; + +namespace OpenSim.Region.Physics.OdePlugin +{ + public class OdeCharacter : PhysicsActor + { + private PhysicsVector _position; + private d.Vector3 _zeroPosition; + private bool _zeroFlag = false; + private bool m_lastUpdateSent = false; + private PhysicsVector _velocity; + private PhysicsVector _target_velocity; + private PhysicsVector _acceleration; + private PhysicsVector m_rotationalVelocity; + private static float PID_D = 3020.0f; + private static float PID_P = 7000.0f; + private static float POSTURE_SERVO = 10000.0f; + public static float CAPSULE_RADIUS = 0.5f; + public float CAPSULE_LENGTH = 0.79f; + private bool 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_alwaysRun = false; + private bool m_hackSentFall = false; + private bool m_hackSentFly = false; + private string m_name = ""; + + private bool[] m_colliderarr = new bool[11]; + private bool[] m_colliderGroundarr = new bool[11]; + + + private bool jumping = false; + //private float gravityAccel; + public IntPtr Body; + private OdeScene _parent_scene; + public IntPtr Shell; + public d.Mass ShellMass; + public bool collidelock = false; + + public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos) + { + _velocity = new PhysicsVector(); + _target_velocity = new PhysicsVector(); + _position = pos; + _acceleration = new PhysicsVector(); + _parent_scene = parent_scene; + + for (int i = 0; i < 11; i++) + { + m_colliderarr[i] = false; + } + + lock (OdeScene.OdeLock) + { + + Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH); + d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f); + Body = d.BodyCreate(parent_scene.world); + d.BodySetMass(Body, ref ShellMass); + d.BodySetPosition(Body, pos.X, pos.Y, pos.Z); + d.GeomSetBody(Shell, Body); + } + m_name = avName; + parent_scene.geom_name_map[Shell] = avName; + parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + public override int PhysicsActorType + { + get { return (int)ActorTypes.Agent; } + set { return; } + } + public override bool SetAlwaysRun + { + get { return m_alwaysRun; } + set { m_alwaysRun = value; } + } + public override bool IsPhysical + { + get { return false; } + set { return; } + } + public override bool ThrottleUpdates + { + get { return false; } + set { return; } + } + public override bool Flying + { + get { return flying; } + set { flying = value; } + } + 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. + + 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; + } + } + public override bool CollidingGround + { + get { return m_iscollidingGround; } + set + { + 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; + } + } + public override bool CollidingObj + { + get { return false; } + set { return; } + } + + public override PhysicsVector Position + { + get { return _position; } + set + { + lock (OdeScene.OdeLock) + { + d.BodySetPosition(Body, value.X, value.Y, value.Z); + _position = value; + } + } + } + public override PhysicsVector RotationalVelocity + { + get { return m_rotationalVelocity; } + set { m_rotationalVelocity = value; } + } + public override PhysicsVector Size + { + get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); } + set + { + lock (OdeScene.OdeLock) + { + PhysicsVector SetSize = value; + float prevCapsule = CAPSULE_LENGTH; + float capsuleradius = CAPSULE_RADIUS; + capsuleradius = 0.2f; + + CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z * 0.43f))); // subtract 43% of the size + d.BodyDestroy(Body); + d.GeomDestroy(Shell); + //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH)); + Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH); + d.MassSetCapsule(out ShellMass, 50.0f, 3, CAPSULE_RADIUS, CAPSULE_LENGTH); + Body = d.BodyCreate(_parent_scene.world); + d.BodySetMass(Body, ref ShellMass); + d.BodySetPosition(Body, _position.X, _position.Y, _position.Z + Math.Abs(CAPSULE_LENGTH - prevCapsule)); + d.GeomSetBody(Shell, Body); + } + _parent_scene.geom_name_map[Shell] = m_name; + _parent_scene.actor_name_map[Shell] = (PhysicsActor)this; + } + } + + public override PrimitiveBaseShape Shape + { + set + { + return; + } + } + + public override PhysicsVector Velocity + { + get { return _velocity; } + set { _target_velocity = value; } + } + + public override bool Kinematic + { + get { return false; } + set { } + } + + public override Quaternion Orientation + { + get { return Quaternion.Identity; } + set { } + } + + public override PhysicsVector Acceleration + { + get { return _acceleration; } + } + + public void SetAcceleration(PhysicsVector accel) + { + _acceleration = accel; + } + + public override void AddForce(PhysicsVector force) + { + + _target_velocity.X += force.X; + _target_velocity.Y += force.Y; + _target_velocity.Z += force.Z; + + //m_lastUpdateSent = false; + } + public void doForce(PhysicsVector force) + { + if (!collidelock) + { + d.BodyAddForce(Body, force.X, force.Y, force.Z); + + // ok -- let's stand up straight! + d.Vector3 feet; + d.Vector3 head; + d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet); + d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head); + float posture = head.Z - feet.Z; + + // restoring force proportional to lack of posture: + float servo = (2.5f - posture) * POSTURE_SERVO; + d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f); + d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f); + //m_lastUpdateSent = false; + + } + + } + public override void SetMomentum(PhysicsVector momentum) + { + + } + + public void Move(float timeStep) + { + // no lock; for now it's only called from within Simulate() + PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + float movementdivisor = 1f; + + if (!m_alwaysRun) + { + movementdivisor = 1.3f; + } + else + { + movementdivisor = 0.8f; + + } + + // if velocity is zero, use position control; otherwise, velocity control + if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding) + { + // keep track of where we stopped. No more slippin' & slidin' + if (!_zeroFlag) + { + _zeroFlag = true; + _zeroPosition = d.BodyGetPosition(Body); + } + d.Vector3 pos = d.BodyGetPosition(Body); + vec.X = (_target_velocity.X - vel.X) * PID_D + (_zeroPosition.X - pos.X) * PID_P; + vec.Y = (_target_velocity.Y - vel.Y) * PID_D + (_zeroPosition.Y - pos.Y) * PID_P; + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + } + } + else + { + + _zeroFlag = false; + if (m_iscolliding || flying) + { + + vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * PID_D; + vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * PID_D; + } + if (m_iscolliding && !flying && _target_velocity.Z > 0.0f) + { + d.Vector3 pos = d.BodyGetPosition(Body); + vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + } + else if (!m_iscolliding && !flying) + { + d.Vector3 pos = d.BodyGetPosition(Body); + if (_target_velocity.X > 0) + { + vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D; + } + if (_target_velocity.Y > 0) + { + vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D; + } + + } + + + if (flying) + { + vec.Z = (_target_velocity.Z - vel.Z) * PID_D; + } + } + if (flying) + { + vec.Z += 10.0f; + } + + + doForce(vec); + } + + public void UpdatePositionAndVelocity() + { + // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! + d.Vector3 vec = d.BodyGetPosition(Body); + + // 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 > 255.95f) vec.X = 255.95f; + if (vec.Y > 255.95f) vec.Y = 255.95f; + + _position.X = vec.X; + _position.Y = vec.Y; + _position.Z = vec.Z; + + if (_zeroFlag) + { + _velocity.X = 0.0f; + _velocity.Y = 0.0f; + _velocity.Z = 0.0f; + if (!m_lastUpdateSent) + { + m_lastUpdateSent = true; + base.RequestPhysicsterseUpdate(); + string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); + int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); + if (primScenAvatarIn == "0") + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in space with no prim. Arr:':" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + else + { + OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in Prim space':" + primScenAvatarIn + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString()); + } + + } + } + else + { + m_lastUpdateSent = false; + vec = d.BodyGetLinearVel(Body); + _velocity.X = (vec.X); + _velocity.Y = (vec.Y); + + _velocity.Z = (vec.Z); + if (_velocity.Z < -6 && !m_hackSentFall) + { + m_hackSentFall = true; + base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else if (flying && !m_hackSentFly) + { + //m_hackSentFly = true; + //base.SendCollisionUpdate(new CollisionEventUpdate()); + } + else + { + m_hackSentFly = false; + m_hackSentFall = false; + } + } + } + + public void Destroy() + { + lock (OdeScene.OdeLock) + { + d.GeomDestroy(Shell); + _parent_scene.geom_name_map.Remove(Shell); + d.BodyDestroy(Body); + } + } + } + +} -- cgit v1.1