/*
* Copyright (c) Contributors, https://github.com/jonc/osboids
* 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
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the OpenSimulator Project nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
using System;
using OpenMetaverse;
using OpenSim.Region.Framework.Scenes;
namespace Flocking
{
public class FlowField
{
private const int BUFFER = 5;
private Scene m_scene;
private float m_startX;
private float m_startY;
private float m_startZ;
private float m_endX;
private float m_endY;
private float m_endZ;
private UUID TERRAIN = UUID.Random ();
private UUID EDGE = UUID.Random ();
private UUID[,,] m_field = new UUID[256, 256, 256]; // field of objects at this position
/// <summary>
/// Initializes a new instance of the <see cref="Flocking.FlowField"/> class.
/// </summary>
/// <param name='scene'>
/// Scene.
/// </param>
/// <param name='centre'>
/// Centre.
/// </param>
/// <param name='width'>
/// Width.
/// </param>
/// <param name='depth'>
/// Depth.
/// </param>
/// <param name='height'>
/// Height.
/// </param>
///
public FlowField (Scene scene, Vector3 centre, int width, int depth, int height)
{
m_scene = scene;
m_startX = Math.Max (BUFFER, centre.X - width / 2f);
m_startY = Math.Max (BUFFER, centre.Y - depth / 2f);
m_startZ = Math.Max (BUFFER, centre.Z - height / 2f);
m_endX = Math.Min (Util.SCENE_SIZE - BUFFER, centre.X + width / 2f);
m_endY = Math.Min (Util.SCENE_SIZE - BUFFER, centre.Y + depth / 2f);
m_endZ = Math.Min (Util.SCENE_SIZE - BUFFER, centre.Z + height / 2f);
// build the flow field over the given bounds
Initialize ();
}
/// <summary>
/// build a flow field on the scene at the specified centre
/// position in the scene and of extent given by width, depth and height.
/// </summary>
public void Initialize ()
{
//fill in the boundaries
for (int x = 0; x < 256; x++) {
for (int y = 0; y < 256; y++) {
m_field [x, y, 0] = EDGE;
m_field [x, y, 255] = EDGE;
}
}
for (int x = 0; x < 256; x++) {
for (int z = 0; z < 256; z++) {
m_field [x, 0, z] = EDGE;
m_field [x, 255, z] = EDGE;
}
}
for (int y = 0; y < 256; y++) {
for (int z = 0; z < 256; z++) {
m_field [0, y, z] = EDGE;
m_field [255, y, z] = EDGE;
}
}
//fill in the terrain
for (int x = 0; x < 256; x++) {
for (int y = 0; y < 256; y++) {
int zMax = Convert.ToInt32 (m_scene.GetGroundHeight (x, y));
for (int z = 1; z < zMax; z++) {
m_field [x, y, z] = TERRAIN;
}
}
}
foreach (SceneObjectGroup sog in m_scene.Entities.GetAllByType<SceneObjectGroup>()) {
//todo: ignore phantom
float fmaxX, fminX, fmaxY, fminY, fmaxZ, fminZ;
int maxX, minX, maxY, minY, maxZ, minZ;
sog.GetAxisAlignedBoundingBoxRaw (out fminX, out fmaxX, out fminY, out fmaxY, out fminZ, out fmaxZ);
Vector3 pos = sog.AbsolutePosition;
minX = Convert.ToInt32 (fminX + pos.X);
maxX = Convert.ToInt32 (fmaxX + pos.X);
minY = Convert.ToInt32 (fminY + pos.Y);
maxY = Convert.ToInt32 (fmaxX + pos.Y);
minZ = Convert.ToInt32 (fminZ + pos.Z);
maxZ = Convert.ToInt32 (fmaxZ + pos.Z);
for (int x = minX; x < maxX; x++) {
for (int y = minY; y < maxY; y++) {
for (int z = minZ; z < maxZ; z++) {
if( inBounds(x,y,z) ) {
m_field [x, y, z] = sog.UUID;
} else {
Console.WriteLine(sog.Name + " OOB at " + sog.AbsolutePosition + " -> " + x + " " + y + " " + z);
}
}
}
}
}
}
private bool inBounds (int x, int y, int z)
{
return x >= 0 && x < 256 && y >= 0 && y < 256 && z >= 0;
}
#if false
public Vector3 AdjustVelocity (Boid boid, float lookAheadDist)
{
Vector3 normVel = Vector3.Normalize (boid.Velocity);
Vector3 loc = boid.Location;
Vector3 inFront = loc + normVel * lookAheadDist;
Vector3 adjustedDestintation = FieldStrength (loc, boid.Size, inFront);
Vector3 newVel = Vector3.Normalize (adjustedDestintation - loc) * Vector3.Mag (boid.Velocity);
float mOrigVel = Vector3.Mag(boid.Velocity);
float mNewVel = Vector3.Mag(newVel);
if( mNewVel != 0f && mNewVel > mOrigVel ) {
newVel *= mOrigVel / mNewVel;
}
return newVel;
}
#endif
public Vector3 FieldStrength (Vector3 currentPos, Vector3 size, Vector3 targetPos)
{
float length = size.X/2;
float width = size.Y/2;
float height = size.Z/2;
//keep us in bounds
targetPos.X = Math.Min( targetPos.X, m_endX - length );
targetPos.X = Math.Max(targetPos.X, m_startX + length);
targetPos.Y = Math.Min( targetPos.Y, m_endY - width );
targetPos.Y = Math.Max(targetPos.Y, m_startY + width);
targetPos.Z = Math.Min( targetPos.Z, m_endZ - height );
targetPos.Z = Math.Max(targetPos.Z, m_startZ + height);
int count = 0;
//now get the field strength at the inbounds position
UUID collider = LookUp (targetPos);
while (collider != UUID.Zero && count < 100) {
count++;
if (collider == TERRAIN) {
// ground height at currentPos and dest averaged
float h1 = m_scene.GetGroundHeight (currentPos.X, currentPos.Y);
float h2 = m_scene.GetGroundHeight (targetPos.X, targetPos.Y);
float h = (h1 + h2) / 2;
targetPos.Z = h + height;
} else if (collider == EDGE) {
//keep us in bounds
targetPos.X = Math.Min( targetPos.X, m_endX - length );
targetPos.X = Math.Max(targetPos.X, m_startX + length);
targetPos.Y = Math.Min( targetPos.Y, m_endY - width );
targetPos.Y = Math.Max(targetPos.Y, m_startY + width);
targetPos.Z = Math.Min( targetPos.Z, m_endZ - height );
targetPos.Z = Math.Max(targetPos.Z, m_startZ + height);
} else {
//we have hit a SOG
SceneObjectGroup sog = m_scene.GetSceneObjectPart(collider).ParentGroup;
if (sog == null) {
Console.WriteLine (collider);
} else {
float sogMinX, sogMinY, sogMinZ, sogMaxX, sogMaxY, sogMaxZ;
sog.GetAxisAlignedBoundingBoxRaw (out sogMinX, out sogMaxX, out sogMinY, out sogMaxY, out sogMinZ, out sogMaxZ);
Vector3 pos = sog.AbsolutePosition;
//keep us out of the sog
// adjust up/down first if necessary
// then turn left or right
if (targetPos.Z > sogMinZ + pos.Z)
targetPos.Z = (sogMinZ + pos.Z) - height;
if (targetPos.Z < sogMaxZ + pos.Z)
targetPos.Z = (sogMaxZ + pos.Z) + height;
if (targetPos.X > sogMinX + pos.X)
targetPos.X = (sogMinX + pos.X) - length;
if (targetPos.Y > sogMinY + pos.Y)
targetPos.Y = (sogMinY + pos.Y) - width;
if (targetPos.X < sogMaxX + pos.X)
targetPos.X = (sogMaxX + pos.X) + length;
if (targetPos.Y < sogMaxY + pos.Y)
targetPos.Y = (sogMaxY + pos.Y) + width;
}
}
// we what is at the new target position
collider = LookUp (targetPos);
}
return targetPos;
}
public UUID LookUp (Vector3 loc)
{
return m_field [(int)loc.X, (int)loc.Y, (int)loc.Z];
}
}
}