/// <summary> /// Called from Simulate /// This is the avatar's movement control + PID Controller /// </summary> /// <param name="timeStep"></param> 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; Vector3 vec = Vector3.Zero; Vector3 vel = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); vel *= 0.25f; 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 Vector3 pos = new Vector3(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); // We don't want linear velocity to cause our avatar to bounce, so we check target Z and actual velocity X, Y // rebound preventing //if (m_target_velocity.Z < 0.025f && m_velocity.X < 0.25f && m_velocity.Y < 0.25f) // m_zeroFlag = true; } 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. Vector3 pos = new Vector3(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) * 10; } } if (m_flying) { // Slight PID correction vec.Z += (((-1 * m_parent_scene.gravityz) * m_mass) * 1.5f); //auto fly height. Kitto Flora //d.Vector3 pos = d.BodyGetPosition(Body); float target_altitude = m_parent_scene.GetTerrainHeightAtXY(m_position.X, m_position.Y) + m_parent_scene.minimumGroundFlightOffset; if (m_position.Z < target_altitude) { vec.Z += (target_altitude - m_position.Z) * PID_P * 5.0f; } } 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); }
private void MoveLinear(float timestep) { if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant { // add drive to body Vector3 addAmount = m_linearMotorDirection / (m_linearMotorTimescale / timestep); 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 / timestep))); //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; btQuaternion rot = m_body.getWorldTransform().getRotation(); Quaternion rotq = new Quaternion(rot.getX(), rot.getY(), rot.getZ(), rot.getW()); // 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; //Rev: bullet does gravity internally grav.Z = -parent_scene.gravityz * objMass * m_VehicleBuoyancy; //parent_scene.gravityz/* * objMass*/ * (1f - m_VehicleBuoyancy); // Preserve the current Z velocity btVector3 pos = m_body.getWorldTransform().getOrigin(); btVector3 newpos = pos; m_dir.Z = m_prim.Velocity.Z; // Preserve the accumulated falling velocity Vector3 posChange = new Vector3(); posChange.X = newpos.getX() - m_lastPositionVector.getX(); posChange.Y = newpos.getY() - m_lastPositionVector.getY(); posChange.Z = newpos.getZ() - m_lastPositionVector.getZ(); btQuaternion Orientation2 = m_body.getWorldTransform().getRotation(); /*if (m_BlockingEndPoint != Vector3.Zero) * { * if (newpos.getX() >= (m_BlockingEndPoint.X - (float)1)) * newpos.setX(newpos.getX() - (posChange.X + 1)); * if (newpos.getY() >= (m_BlockingEndPoint.Y - (float)1)) * newpos.setY(newpos.getY() - (posChange.Y + 1)); * if (newpos.getZ() >= (m_BlockingEndPoint.Z - (float)1)) * newpos.setZ(newpos.getZ() - (posChange.Z + 1)); * if (newpos.getX() <= 0) * newpos.setX(newpos.getX() + (posChange.X + 1)); * if (newpos.getY() <= 0) * newpos.setY(newpos.getY() + (posChange.Y + 1)); * } */ if (newpos.getZ() < parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY())) { newpos.setZ(parent_scene.GetTerrainHeightAtXY(newpos.getX(), newpos.getY()) + 2); } // Check if hovering if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) { float diff = (newpos.getZ() - m_VhoverTargetHeight); // We should hover, get the target height if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0) { m_VhoverTargetHeight = parent_scene.GetWaterLevel() + m_VhoverHeight; } if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) { m_VhoverTargetHeight = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()) + 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 (newpos.getZ() > m_VhoverTargetHeight) { m_VhoverTargetHeight = newpos.getZ(); } } if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) { if (diff > .2 || diff < -.2) { newpos.setValue(newpos.getX(), newpos.getY(), m_VhoverTargetHeight); btTransform trans = new btTransform(Orientation2, newpos); m_body.setWorldTransform(trans); } } else { // Replace Vertical speed with correction figure if significant if (Math.Abs(diff) > 0.01f) { m_dir.Z = -((diff * timestep * 50.0f) / m_VhoverTimescale); } else { m_dir.Z = 0f; } } } /*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 = parent_scene.GetTerrainHeightAtXY(pos.getX(), pos.getY()); * float postemp = (pos.getZ() - 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 = new btVector3(m_prim.Position.X, m_prim.Position.Y, m_prim.Position.Z); // Apply velocity //if(m_dir != Vector3.Zero) // m_body.setLinearVelocity(new btVector3(m_dir.X, m_dir.Y, m_dir.Z)); m_body.applyCentralImpulse(new btVector3(m_dir.X, m_dir.Y, m_dir.Z)); // apply gravity force //m_body.applyCentralImpulse(new btVector3(0, 0, 9.8f)); /*ector3 newpos2 = new Vector3(newpos.getX(), newpos.getY(), newpos.getZ()); * if (newpos2.X != m_prim.Position.X || newpos2.Y != m_prim.Position.Y || newpos2.Z != m_prim.Position.Z) * { * btTransform trans = new btTransform(Orientation2, newpos); * m_body.setWorldTransform(trans); * }*/ // apply friction Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / timestep); m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; }
/// <summary> /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. /// </summary> 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! Vector3 vec = new Vector3(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 < -10.0f) { vec.X = 0.0f; } if (vec.Y < -10.0f) { vec.Y = 0.0f; } if (vec.X > m_parent_scene.m_region.RegionSizeX + 10.2f) { vec.X = m_parent_scene.m_region.RegionSizeX + 10.2f; } if (vec.Y > m_parent_scene.m_region.RegionSizeY + 10.2f) { vec.Y = m_parent_scene.m_region.RegionSizeY + 10.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(); } //Tell any listeners that we've stopped base.TriggerMovementUpdate(); } else { m_lastUpdateSent = false; vec = new Vector3(tempVector2.getX(), tempVector2.getY(), tempVector2.getZ()); m_velocity.X = (vec.X); m_velocity.Y = (vec.Y); m_velocity.Z = (vec.Z); //m_log.Debug(m_target_velocity); 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; } const float VELOCITY_TOLERANCE = 0.001f; const float POSITION_TOLERANCE = 0.05f; //Check to see whether we need to trigger the significant movement method in the presence if (!RotationalVelocity.ApproxEquals(m_lastRotationalVelocity, VELOCITY_TOLERANCE) || !Velocity.ApproxEquals(m_lastVelocity, VELOCITY_TOLERANCE) || !Position.ApproxEquals(m_lastPosition, POSITION_TOLERANCE)) { // Update the "last" values m_lastPosition = Position; m_lastRotationalVelocity = RotationalVelocity; m_lastVelocity = Velocity; base.RequestPhysicsterseUpdate(); base.TriggerSignificantMovement(); } //Tell any listeners about the new info base.TriggerMovementUpdate(); } if (Body != null) { if (Body.getFriction() < 0.9f) { Body.setFriction(0.9f); } } //if (Body != null) // Body.clearForces(); }
/// <summary> /// Updates the reported position and velocity. This essentially sends the data up to ScenePresence. /// </summary> 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 < -10.0f) { vec.X = 0.0f; } if (vec.Y < -10.0f) { vec.Y = 0.0f; } if (vec.X > (int)Constants.RegionSize + 10.2f) { vec.X = (int)Constants.RegionSize + 10.2f; } if (vec.Y > (int)Constants.RegionSize + 10.2f) { vec.Y = (int)Constants.RegionSize + 10.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); //m_log.Debug(m_target_velocity); 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(); }