From 70d5b1c34cf2eb6621f383169fdee03966850762 Mon Sep 17 00:00:00 2001 From: Jeff Ames Date: Mon, 4 Jan 2010 06:10:45 +0900 Subject: Formatting cleanup. Add copyright headers. --- OpenSim/Region/Physics/OdePlugin/ODEPrim.cs | 562 ++++++++++++++-------------- 1 file changed, 282 insertions(+), 280 deletions(-) (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs') diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs index 3eb3b28..973aa84 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs @@ -1,5 +1,7 @@ -/* Copyright (c) Contributors, http://opensimulator.org/ +/* + * 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 @@ -93,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_targetHoverHeight; private float m_groundHeight; private float m_waterHeight; - private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. // private float m_tensor = 5f; private int body_autodisable_frames = 20; @@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_taintselected = value; m_isSelected = value; } - if(m_isSelected) disableBodySoft(); + if (m_isSelected) disableBodySoft(); } } @@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin { prev_geom = prim_geom; prim_geom = geom; -//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); +//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); if (prim_geom != IntPtr.Zero) { d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); @@ -314,7 +316,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (_parent != null && _parent is OdePrim) { OdePrim parent = (OdePrim)_parent; -//Console.WriteLine("SetGeom calls ChildSetGeom"); +//Console.WriteLine("SetGeom calls ChildSetGeom"); parent.ChildSetGeom(this); } } @@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin { d.BodyEnable(Body); if (m_vehicle.Type != Vehicle.TYPE_NONE) - m_vehicle.Enable(Body, _parent_scene); + m_vehicle.Enable(Body, _parent_scene); } m_disabled = false; @@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin d.BodySetAutoDisableSteps(Body, body_autodisable_frames); // disconnect from world gravity so we can apply buoyancy - d.BodySetGravityMode (Body, false); + d.BodySetGravityMode (Body, false); m_interpenetrationcount = 0; m_collisionscore = 0; @@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void ProcessTaints(float timestep) { -//Console.WriteLine("ProcessTaints for " + m_primName ); +//Console.WriteLine("ProcessTaints for " + m_primName); if (m_taintadd) { changeadd(timestep); @@ -880,24 +882,24 @@ namespace OpenSim.Region.Physics.OdePlugin if (prim_geom != IntPtr.Zero) { - if (!_position.ApproxEquals(m_taintposition, 0f)) - changemove(timestep); - - if (m_taintrot != _orientation) - { - if(childPrim && IsPhysical) // For physical child prim... - { - rotate(timestep); - // KF: ODE will also rotate the parent prim! - // so rotate the root back to where it was - OdePrim parent = (OdePrim)_parent; - parent.rotate(timestep); + if (!_position.ApproxEquals(m_taintposition, 0f)) + changemove(timestep); + + if (m_taintrot != _orientation) + { + if (childPrim && IsPhysical) // For physical child prim... + { + rotate(timestep); + // KF: ODE will also rotate the parent prim! + // so rotate the root back to where it was + OdePrim parent = (OdePrim)_parent; + parent.rotate(timestep); } else { - //Just rotate the prim - rotate(timestep); - } + //Just rotate the prim + rotate(timestep); + } } // @@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin // destroy link else if (_parent != null && m_taintparent == null) { -//Console.WriteLine(" changelink B"); +//Console.WriteLine(" changelink B"); if (_parent is OdePrim) { @@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin // prim is the child public void ParentPrim(OdePrim prim) { -//Console.WriteLine("ParentPrim " + m_primName); +//Console.WriteLine("ParentPrim " + m_primName); if (this.m_localID != prim.m_localID) { if (Body == IntPtr.Zero) @@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin { if (!childrenPrim.Contains(prim)) { -//Console.WriteLine("childrenPrim.Add " + prim); +//Console.WriteLine("childrenPrim.Add " + prim); childrenPrim.Add(prim); foreach (OdePrim prm in childrenPrim) @@ -1080,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); continue; } -//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); +//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); @@ -1128,7 +1130,7 @@ namespace OpenSim.Region.Physics.OdePlugin m_collisionCategories |= CollisionCategories.Body; m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); -//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); +//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); //Console.WriteLine(" Post GeomSetCategoryBits 2"); d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); @@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildSetGeom calls ParentPrim"); +//Console.WriteLine("ChildSetGeom calls ParentPrim"); ParentPrim(prm); } } @@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (childrenPrim) { - //Console.WriteLine("childrenPrim.Remove " + odePrim); + //Console.WriteLine("childrenPrim.Remove " + odePrim); childrenPrim.Remove(odePrim); } @@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin { foreach (OdePrim prm in childrenPrim) { -//Console.WriteLine("ChildDelink calls ParentPrim"); +//Console.WriteLine("ChildDelink calls ParentPrim"); ParentPrim(prm); } } @@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) { -//Console.WriteLine("CreateGeom:"); +//Console.WriteLine("CreateGeom:"); if (_mesh != null) { setMesh(_parent_scene, _mesh); @@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 2"); +//Console.WriteLine(" CreateGeom 2"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 3"); +//Console.WriteLine(" CreateGeom 3"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin _parent_scene.waitForSpaceUnlock(m_targetSpace); try { -//Console.WriteLine(" CreateGeom 4"); +//Console.WriteLine(" CreateGeom 4"); SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); } catch (AccessViolationException) @@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin lock (_parent_scene.OdeLock) { -//Console.WriteLine("changeadd 1"); +//Console.WriteLine("changeadd 1"); CreateGeom(m_targetSpace, _mesh); if (prim_geom != IntPtr.Zero) @@ -1508,7 +1510,7 @@ namespace OpenSim.Region.Physics.OdePlugin if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) { // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? -Console.WriteLine(" JointCreateFixed"); +Console.WriteLine(" JointCreateFixed"); m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); d.JointAttach(m_linkJoint, Body, odParent.Body); d.JointSetFixed(m_linkJoint); @@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed"); float fz = 0; - if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. - { - if (m_vehicle.Type != Vehicle.TYPE_NONE) - { - // 'VEHICLES' are dealt with in ODEDynamics.cs - m_vehicle.Step(timestep, _parent_scene); - } - else - { -//Console.WriteLine("Move " + m_primName); - if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 - // NON-'VEHICLES' are dealt with here - if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) - { - d.Vector3 avel2 = d.BodyGetAngularVel(Body); - if (m_angularlock.X == 1) - avel2.X = 0; - if (m_angularlock.Y == 1) - avel2.Y = 0; - if (m_angularlock.Z == 1) - avel2.Z = 0; - d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); - } - //float PID_P = 900.0f; - - float m_mass = CalculateMass(); - -// fz = 0f; + if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. + { + if (m_vehicle.Type != Vehicle.TYPE_NONE) + { + // 'VEHICLES' are dealt with in ODEDynamics.cs + m_vehicle.Step(timestep, _parent_scene); + } + else + { +//Console.WriteLine("Move " + m_primName); + if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 + // NON-'VEHICLES' are dealt with here + if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) + { + d.Vector3 avel2 = d.BodyGetAngularVel(Body); + if (m_angularlock.X == 1) + avel2.X = 0; + if (m_angularlock.Y == 1) + avel2.Y = 0; + if (m_angularlock.Z == 1) + avel2.Z = 0; + d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); + } + //float PID_P = 900.0f; + + float m_mass = CalculateMass(); + +// fz = 0f; //m_log.Info(m_collisionFlags.ToString()); - - //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. - // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? - // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up - // gravityz multiplier = 1 - m_buoyancy - fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; - - if (m_usePID) - { -//Console.WriteLine("PID " + m_primName); - // KF - this is for object move? eg. llSetPos() ? - //if (!d.BodyIsEnabled(Body)) - //d.BodySetForce(Body, 0f, 0f, 0f); - // If we're using the PID controller, then we have no gravity - //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... - fz = 0f; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1) && (m_PIDTau != 0)) - { - //PID_G = PID_G / m_PIDTau; - m_PIDTau = 1; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } - //PidStatus = true; - - // PhysicsVector vec = new PhysicsVector(); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - d.Vector3 pos = d.BodyGetPosition(Body); - _target_velocity = + + //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. + // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? + // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up + // gravityz multiplier = 1 - m_buoyancy + fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; + + if (m_usePID) + { +//Console.WriteLine("PID " + m_primName); + // KF - this is for object move? eg. llSetPos() ? + //if (!d.BodyIsEnabled(Body)) + //d.BodySetForce(Body, 0f, 0f, 0f); + // If we're using the PID controller, then we have no gravity + //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... + fz = 0f; + + // no lock; for now it's only called from within Simulate() + + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1) && (m_PIDTau != 0)) + { + //PID_G = PID_G / m_PIDTau; + m_PIDTau = 1; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + //PidStatus = true; + + // PhysicsVector vec = new PhysicsVector(); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + d.Vector3 pos = d.BodyGetPosition(Body); + _target_velocity = new Vector3( - (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), - (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); - //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); - //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; - d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); - d.BodySetLinearVel(Body, 0, 0, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fx = ((_target_velocity.X) - vel.X) * (PID_D); - fy = ((_target_velocity.Y) - vel.Y) * (PID_D); - - // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; - - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } // end if (m_usePID) - - // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller - if (m_useHoverPID && !m_usePID) - { -//Console.WriteLine("Hover " + m_primName); - - // If we're using the PID controller, then we have no gravity - fz = (-1 * _parent_scene.gravityz) * m_mass; - - // no lock; for now it's only called from within Simulate() - - // If the PID Controller isn't active then we set our force - // calculating base velocity to the current position - - if ((m_PIDTau < 1)) - { - PID_G = PID_G / m_PIDTau; - } - - if ((PID_G - m_PIDTau) <= 0) - { - PID_G = m_PIDTau + 1; - } + (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), + (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); + //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); + //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; + d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); + d.BodySetLinearVel(Body, 0, 0, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fx = ((_target_velocity.X) - vel.X) * (PID_D); + fy = ((_target_velocity.Y) - vel.Y) * (PID_D); + + // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; + + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } // end if (m_usePID) + + // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller + if (m_useHoverPID && !m_usePID) + { +//Console.WriteLine("Hover " + m_primName); + // If we're using the PID controller, then we have no gravity + fz = (-1 * _parent_scene.gravityz) * m_mass; + + // no lock; for now it's only called from within Simulate() - // Where are we, and where are we headed? - d.Vector3 pos = d.BodyGetPosition(Body); - d.Vector3 vel = d.BodyGetLinearVel(Body); - - - // Non-Vehicles have a limited set of Hover options. - // determine what our target height really is based on HoverType - switch (m_PIDHoverType) - { - case PIDHoverType.Ground: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - break; - case PIDHoverType.GroundAndWater: - m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); - m_waterHeight = _parent_scene.GetWaterLevel(); - if (m_groundHeight > m_waterHeight) - { - m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; - } - else - { - m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; - } - break; - - } // end switch (m_PIDHoverType) - - - _target_velocity = + // If the PID Controller isn't active then we set our force + // calculating base velocity to the current position + + if ((m_PIDTau < 1)) + { + PID_G = PID_G / m_PIDTau; + } + + if ((PID_G - m_PIDTau) <= 0) + { + PID_G = m_PIDTau + 1; + } + + + // Where are we, and where are we headed? + d.Vector3 pos = d.BodyGetPosition(Body); + d.Vector3 vel = d.BodyGetLinearVel(Body); + + + // Non-Vehicles have a limited set of Hover options. + // determine what our target height really is based on HoverType + switch (m_PIDHoverType) + { + case PIDHoverType.Ground: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + break; + case PIDHoverType.GroundAndWater: + m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); + m_waterHeight = _parent_scene.GetWaterLevel(); + if (m_groundHeight > m_waterHeight) + { + m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; + } + else + { + m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; + } + break; + + } // end switch (m_PIDHoverType) + + + _target_velocity = new Vector3(0.0f, 0.0f, - (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) - ); - - // if velocity is zero, use position control; otherwise, velocity control - - if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) - { - // keep track of where we stopped. No more slippin' & slidin' - - // We only want to deactivate the PID Controller if we think we want to have our surrogate - // react to the physics scene by moving it's position. - // Avatar to Avatar collisions - // Prim to avatar collisions - - d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); - d.BodySetLinearVel(Body, vel.X, vel.Y, 0); - d.BodyAddForce(Body, 0, 0, fz); - return; - } - else - { - _zeroFlag = false; - - // We're flying and colliding with something - fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); - } - } - - fx *= m_mass; - fy *= m_mass; - //fz *= m_mass; - - fx += m_force.X; - fy += m_force.Y; - fz += m_force.Z; - - //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); - if (fx != 0 || fy != 0 || fz != 0) - { - //m_taintdisable = true; - //base.RaiseOutOfBounds(Position); - //d.BodySetLinearVel(Body, fx, fy, 0f); - if (!d.BodyIsEnabled(Body)) - { - // A physical body at rest on a surface will auto-disable after a while, - // this appears to re-enable it incase the surface it is upon vanishes, - // and the body should fall again. - d.BodySetLinearVel(Body, 0f, 0f, 0f); - d.BodySetForce(Body, 0, 0, 0); - enableBodySoft(); - } - - // 35x10 = 350n times the mass per second applied maximum. - float nmax = 35f * m_mass; - float nmin = -35f * m_mass; + (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) + ); + + // if velocity is zero, use position control; otherwise, velocity control + + if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) + { + // keep track of where we stopped. No more slippin' & slidin' + + // We only want to deactivate the PID Controller if we think we want to have our surrogate + // react to the physics scene by moving it's position. + // Avatar to Avatar collisions + // Prim to avatar collisions + + d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); + d.BodySetLinearVel(Body, vel.X, vel.Y, 0); + d.BodyAddForce(Body, 0, 0, fz); + return; + } + else + { + _zeroFlag = false; + + // We're flying and colliding with something + fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); + } + } + + fx *= m_mass; + fy *= m_mass; + //fz *= m_mass; + + fx += m_force.X; + fy += m_force.Y; + fz += m_force.Z; + + //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); + if (fx != 0 || fy != 0 || fz != 0) + { + //m_taintdisable = true; + //base.RaiseOutOfBounds(Position); + //d.BodySetLinearVel(Body, fx, fy, 0f); + if (!d.BodyIsEnabled(Body)) + { + // A physical body at rest on a surface will auto-disable after a while, + // this appears to re-enable it incase the surface it is upon vanishes, + // and the body should fall again. + d.BodySetLinearVel(Body, 0f, 0f, 0f); + d.BodySetForce(Body, 0, 0, 0); + enableBodySoft(); + } + + // 35x10 = 350n times the mass per second applied maximum. + float nmax = 35f * m_mass; + float nmin = -35f * m_mass; - if (fx > nmax) - fx = nmax; - if (fx < nmin) - fx = nmin; - if (fy > nmax) - fy = nmax; - if (fy < nmin) - fy = nmin; - d.BodyAddForce(Body, fx, fy, fz); -//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); - } - } + if (fx > nmax) + fx = nmax; + if (fx < nmin) + fx = nmin; + if (fy > nmax) + fy = nmax; + if (fy < nmin) + fy = nmin; + d.BodyAddForce(Body, fx, fy, fz); +//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); + } + } } else - { // is not physical, or is not a body or is selected + { // is not physical, or is not a body or is selected // _zeroPosition = d.BodyGetPosition(Body); return; -//Console.WriteLine("Nothing " + m_primName); +//Console.WriteLine("Nothing " + m_primName); } } @@ -1815,18 +1817,18 @@ Console.WriteLine(" JointCreateFixed"); myrot.W = _orientation.W; if (Body != IntPtr.Zero) { - // KF: If this is a root prim do BodySet + // KF: If this is a root prim do BodySet d.BodySetQuaternion(Body, ref myrot); - if (m_isphysical) - { + if (m_isphysical) + { if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) - createAMotor(m_angularlock); - } - } - else - { - // daughter prim, do Geom set - d.GeomSetQuaternion(prim_geom, ref myrot); + createAMotor(m_angularlock); + } + } + else + { + // daughter prim, do Geom set + d.GeomSetQuaternion(prim_geom, ref myrot); } resetCollisionAccounting(); @@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed"); m_log.Error("[PHYSICS]: PrimGeom dead"); } } -//Console.WriteLine("changePhysicsStatus for " + m_primName ); +//Console.WriteLine("changePhysicsStatus for " + m_primName); changeadd(2f); } if (childPrim) @@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changesize 2"); +//Console.WriteLine("changesize 2"); CreateGeom(m_targetSpace, _mesh); } @@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed"); else { _mesh = null; -//Console.WriteLine("changeshape"); +//Console.WriteLine("changeshape"); CreateGeom(m_targetSpace, null); } @@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed"); if (QuaternionIsFinite(value)) { _orientation = value; - } + } else m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); @@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed"); //outofBounds = true; } - //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); -//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); + //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); +//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) @@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed"); && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large { _zeroFlag = true; -//Console.WriteLine("ZFT 2"); +//Console.WriteLine("ZFT 2"); m_throttleUpdates = false; } else -- cgit v1.1