aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim')
-rw-r--r--OpenSim/Region/Framework/Scenes/ScenePresence.cs214
1 files changed, 116 insertions, 98 deletions
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index a508813..06b27ca 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -1483,105 +1483,8 @@ namespace OpenSim.Region.Framework.Scenes
1483 i++; 1483 i++;
1484 } 1484 }
1485 1485
1486 //Paupaw:Do Proper PID for Autopilot here 1486 if (DoMoveToPositionUpdate(ref agent_control_v3, bodyRotation, bResetMoveToPosition, bAllowUpdateMoveToPosition))
1487 if (bResetMoveToPosition)
1488 {
1489 m_moveToPositionTarget = Vector3.Zero;
1490 m_moveToPositionInProgress = false;
1491 update_movementflag = true; 1487 update_movementflag = true;
1492 bAllowUpdateMoveToPosition = false;
1493 }
1494
1495 m_log.DebugFormat(
1496 "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}",
1497 bAllowUpdateMoveToPosition, m_moveToPositionInProgress, m_autopilotMoving);
1498
1499 if (bAllowUpdateMoveToPosition && (m_moveToPositionInProgress && !m_autopilotMoving))
1500 {
1501 double distanceToTarget = Util.GetDistanceTo(AbsolutePosition, m_moveToPositionTarget);
1502// m_log.DebugFormat(
1503// "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
1504// Name, AbsolutePosition, m_moveToPositionTarget, distanceToTarget);
1505
1506 // Check the error term of the current position in relation to the target position
1507 if (distanceToTarget <= 1)
1508 {
1509 // We are close enough to the target
1510 m_moveToPositionTarget = Vector3.Zero;
1511 m_moveToPositionInProgress = false;
1512 update_movementflag = true;
1513 }
1514 else
1515 {
1516 try
1517 {
1518 // move avatar in 2D at one meter/second towards target, in avatar coordinate frame.
1519 // This movement vector gets added to the velocity through AddNewMovement().
1520 // Theoretically we might need a more complex PID approach here if other
1521 // unknown forces are acting on the avatar and we need to adaptively respond
1522 // to such forces, but the following simple approach seems to works fine.
1523 Vector3 LocalVectorToTarget3D =
1524 (m_moveToPositionTarget - AbsolutePosition) // vector from cur. pos to target in global coords
1525 * Matrix4.CreateFromQuaternion(Quaternion.Inverse(bodyRotation)); // change to avatar coords
1526 // Ignore z component of vector
1527 Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
1528 LocalVectorToTarget2D.Normalize();
1529 agent_control_v3 += LocalVectorToTarget2D;
1530
1531 // update avatar movement flags. the avatar coordinate system is as follows:
1532 //
1533 // +X (forward)
1534 //
1535 // ^
1536 // |
1537 // |
1538 // |
1539 // |
1540 // (left) +Y <--------o--------> -Y
1541 // avatar
1542 // |
1543 // |
1544 // |
1545 // |
1546 // v
1547 // -X
1548 //
1549
1550 // based on the above avatar coordinate system, classify the movement into
1551 // one of left/right/back/forward.
1552 if (LocalVectorToTarget2D.Y > 0)//MoveLeft
1553 {
1554 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
1555 //AgentControlFlags
1556 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
1557 update_movementflag = true;
1558 }
1559 else if (LocalVectorToTarget2D.Y < 0) //MoveRight
1560 {
1561 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
1562 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
1563 update_movementflag = true;
1564 }
1565 if (LocalVectorToTarget2D.X < 0) //MoveBack
1566 {
1567 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
1568 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
1569 update_movementflag = true;
1570 }
1571 else if (LocalVectorToTarget2D.X > 0) //Move Forward
1572 {
1573 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
1574 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
1575 update_movementflag = true;
1576 }
1577 }
1578 catch (Exception e)
1579 {
1580 //Avoid system crash, can be slower but...
1581 m_log.DebugFormat("Crash! {0}", e.ToString());
1582 }
1583 }
1584 }
1585 } 1488 }
1586 1489
1587 // Cause the avatar to stop flying if it's colliding 1490 // Cause the avatar to stop flying if it's colliding
@@ -1628,6 +1531,121 @@ namespace OpenSim.Region.Framework.Scenes
1628 m_scene.StatsReporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS)); 1531 m_scene.StatsReporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS));
1629 } 1532 }
1630 1533
1534 /// <summary>
1535 /// Process moving the avatar if a position has been set.
1536 /// </summary>
1537 /// <param value="agent_control_v3">Cumulative agent movement that this method will update.</param>
1538 /// <param value="bodyRotation">New body rotation of the avatar.</param>
1539 /// <param value="reset">If true, clear the move to position</param>
1540 /// <param value="allowUpdate">If true, allow the update in principle.</param>
1541 /// <returns>True if movement has been updated in some way. False otherwise.</returns>
1542 protected bool DoMoveToPositionUpdate(
1543 ref Vector3 agent_control_v3, Quaternion bodyRotation, bool reset, bool allowUpdate)
1544 {
1545 bool updated = false;
1546
1547 //Paupaw:Do Proper PID for Autopilot here
1548 if (reset)
1549 {
1550 m_moveToPositionTarget = Vector3.Zero;
1551 m_moveToPositionInProgress = false;
1552 updated = true;
1553 }
1554
1555 m_log.DebugFormat(
1556 "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}",
1557 allowUpdate, m_moveToPositionInProgress, m_autopilotMoving);
1558
1559 if (allowUpdate && (m_moveToPositionInProgress && !m_autopilotMoving))
1560 {
1561 double distanceToTarget = Util.GetDistanceTo(AbsolutePosition, m_moveToPositionTarget);
1562// m_log.DebugFormat(
1563// "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
1564// Name, AbsolutePosition, m_moveToPositionTarget, distanceToTarget);
1565
1566 // Check the error term of the current position in relation to the target position
1567 if (distanceToTarget <= 1)
1568 {
1569 // We are close enough to the target
1570 m_moveToPositionTarget = Vector3.Zero;
1571 m_moveToPositionInProgress = false;
1572 updated = true;
1573 }
1574 else
1575 {
1576 try
1577 {
1578 // move avatar in 2D at one meter/second towards target, in avatar coordinate frame.
1579 // This movement vector gets added to the velocity through AddNewMovement().
1580 // Theoretically we might need a more complex PID approach here if other
1581 // unknown forces are acting on the avatar and we need to adaptively respond
1582 // to such forces, but the following simple approach seems to works fine.
1583 Vector3 LocalVectorToTarget3D =
1584 (m_moveToPositionTarget - AbsolutePosition) // vector from cur. pos to target in global coords
1585 * Matrix4.CreateFromQuaternion(Quaternion.Inverse(bodyRotation)); // change to avatar coords
1586 // Ignore z component of vector
1587 Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
1588 LocalVectorToTarget2D.Normalize();
1589 agent_control_v3 += LocalVectorToTarget2D;
1590
1591 // update avatar movement flags. the avatar coordinate system is as follows:
1592 //
1593 // +X (forward)
1594 //
1595 // ^
1596 // |
1597 // |
1598 // |
1599 // |
1600 // (left) +Y <--------o--------> -Y
1601 // avatar
1602 // |
1603 // |
1604 // |
1605 // |
1606 // v
1607 // -X
1608 //
1609
1610 // based on the above avatar coordinate system, classify the movement into
1611 // one of left/right/back/forward.
1612 if (LocalVectorToTarget2D.Y > 0)//MoveLeft
1613 {
1614 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
1615 //AgentControlFlags
1616 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
1617 updated = true;
1618 }
1619 else if (LocalVectorToTarget2D.Y < 0) //MoveRight
1620 {
1621 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
1622 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
1623 updated = true;
1624 }
1625 if (LocalVectorToTarget2D.X < 0) //MoveBack
1626 {
1627 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
1628 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
1629 updated = true;
1630 }
1631 else if (LocalVectorToTarget2D.X > 0) //Move Forward
1632 {
1633 m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
1634 AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
1635 updated = true;
1636 }
1637 }
1638 catch (Exception e)
1639 {
1640 //Avoid system crash, can be slower but...
1641 m_log.DebugFormat("Crash! {0}", e.ToString());
1642 }
1643 }
1644 }
1645
1646 return updated;
1647 }
1648
1631// public void DoAutoPilot(uint not_used, Vector3 Pos, IClientAPI remote_client) 1649// public void DoAutoPilot(uint not_used, Vector3 Pos, IClientAPI remote_client)
1632// { 1650// {
1633// m_autopilotMoving = true; 1651// m_autopilotMoving = true;