341 lines
10 KiB
C#
341 lines
10 KiB
C#
/*
|
|
* 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 System.Collections.Generic;
|
|
using OpenMetaverse;
|
|
|
|
namespace Flocking
|
|
{
|
|
public class Boid
|
|
{
|
|
private string m_id;
|
|
|
|
private Vector3 m_loc;
|
|
private Quaternion m_rotation;
|
|
private Vector3 m_vel;
|
|
private Vector3 m_acc;
|
|
private Random m_rndnums = new Random (Environment.TickCount);
|
|
private float m_tolerance; // how close can we get to things witout being edgy
|
|
private float m_maxForce; // Maximum steering force
|
|
private float m_maxSpeed; // Maximum speed
|
|
|
|
private float m_width = 255f;
|
|
private float m_height = 255f;
|
|
|
|
private float m_neighborDist = 25.0f;
|
|
private float m_desiredSeparation = 20.0f;
|
|
|
|
|
|
/// <summary>
|
|
/// Initializes a new instance of the <see cref="Flocking.Boid"/> class.
|
|
/// </summary>
|
|
/// <param name='l'>
|
|
/// L. the initial position of this boid
|
|
/// </param>
|
|
/// <param name='ms'>
|
|
/// Ms. max speed this boid can attain
|
|
/// </param>
|
|
/// <param name='mf'>
|
|
/// Mf. max force / acceleration this boid can extert
|
|
/// </param>
|
|
public Boid (string id, float ms, float mf)
|
|
{
|
|
m_id = id;
|
|
m_acc = Vector3.Zero;
|
|
m_vel = new Vector3 (m_rndnums.Next (-1, 1), m_rndnums.Next (-1, 1), m_rndnums.Next (-1, 1));
|
|
|
|
m_tolerance = 2.0f;
|
|
m_maxSpeed = ms;
|
|
m_maxForce = mf;
|
|
}
|
|
|
|
public Vector3 Location {
|
|
get { return m_loc;}
|
|
set { m_loc = value; }
|
|
}
|
|
|
|
public Vector3 Velocity {
|
|
get { return m_vel;}
|
|
}
|
|
|
|
public Quaternion Rotation {
|
|
get { return m_rotation; }
|
|
}
|
|
|
|
public String Id {
|
|
get {return m_id;}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Moves our boid in the scene relative to the rest of the flock.
|
|
/// </summary>
|
|
/// <param name='boids'>
|
|
/// Boids. all the other chaps in the scene
|
|
/// </param>
|
|
public void MoveInSceneRelativeToFlock (List<Boid> boids)
|
|
{
|
|
Vector3 previousLoc = new Vector3( m_loc );
|
|
Flock (boids);
|
|
UpdatePositionInScene ();
|
|
AvoidBorders ();
|
|
|
|
m_rotation = Vector3.RotationBetween( previousLoc, m_loc );
|
|
|
|
//render();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move within our flock
|
|
///
|
|
/// We accumulate a new acceleration each time based on three rules
|
|
/// these are:
|
|
/// our separation from our closest neighbours,
|
|
/// our desire to keep travelling within the local flock,
|
|
/// our desire to move towards the flock centre
|
|
///
|
|
/// </summary>
|
|
void Flock (List<Boid> boids)
|
|
{
|
|
|
|
// calc the force vectors on this boid
|
|
Vector3 sep = Separate (boids); // Separation
|
|
Vector3 ali = Align (boids); // Alignment
|
|
Vector3 coh = Cohesion (boids); // Cohesion
|
|
|
|
// Arbitrarily weight these forces
|
|
//TODO: expose these consts
|
|
sep *= 1.5f; //.mult(1.5);
|
|
//ali.mult(1.0);
|
|
ali *= 1.0f;
|
|
//coh.mult(1.0);
|
|
coh *= 1.0f;
|
|
|
|
// Add the force vectors to the current acceleration of the boid
|
|
//acc.add(sep);
|
|
m_acc += sep;
|
|
//acc.add(ali);
|
|
m_acc += ali;
|
|
//acc.add(coh);
|
|
m_acc += coh;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Method to update our location within the scene.
|
|
/// update our location in the world based on our
|
|
/// current location, velocity and acceleration
|
|
/// taking into account our max speed
|
|
///
|
|
/// </summary>
|
|
void UpdatePositionInScene ()
|
|
{
|
|
// Update velocity
|
|
//vel.add(acc);
|
|
m_vel += m_acc;
|
|
// Limit speed
|
|
//m_vel.limit(maxspeed);
|
|
m_vel = Util.Limit (m_vel, m_maxSpeed);
|
|
m_loc += m_vel;
|
|
// Reset accelertion to 0 each cycle
|
|
m_acc *= 0.0f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Seek the specified target. Move into that flock
|
|
/// Accelerate us towards where we want to go
|
|
/// </summary>
|
|
/// <param name='target'>
|
|
/// Target. the position within the flock we would like to achieve
|
|
/// </param>
|
|
void Seek (Vector3 target)
|
|
{
|
|
m_acc += Steer (target, false);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Arrive the specified target. Slow us down, as we are almost there
|
|
/// </summary>
|
|
/// <param name='target'>
|
|
/// Target. the flock we would like to think ourselves part of
|
|
/// </param>
|
|
void arrive (Vector3 target)
|
|
{
|
|
m_acc += Steer (target, true);
|
|
}
|
|
|
|
/// A method that calculates a steering vector towards a target
|
|
/// Takes a second argument, if true, it slows down as it approaches the target
|
|
Vector3 Steer (Vector3 target, bool slowdown)
|
|
{
|
|
Vector3 steer; // The steering vector
|
|
Vector3 desired = Vector3.Subtract(target, m_loc); // A vector pointing from the location to the target
|
|
float d = desired.Length (); // Distance from the target is the magnitude of the vector
|
|
// If the distance is greater than 0, calc steering (otherwise return zero vector)
|
|
if (d > 0) {
|
|
// Normalize desired
|
|
desired.Normalize ();
|
|
// Two options for desired vector magnitude (1 -- based on distance, 2 -- maxspeed)
|
|
if ((slowdown) && (d < 100.0f)) {
|
|
desired *= (m_maxSpeed * (d / 100.0f)); // This damping is somewhat arbitrary
|
|
} else {
|
|
desired *= m_maxSpeed;
|
|
}
|
|
// Steering = Desired minus Velocity
|
|
//steer = target.sub(desired,m_vel);
|
|
steer = Vector3.Subtract (desired, m_vel);
|
|
//steer.limit(maxforce); // Limit to maximum steering force
|
|
steer = Util.Limit (steer, m_maxForce);
|
|
} else {
|
|
steer = Vector3.Zero;
|
|
}
|
|
return steer;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Borders this instance.
|
|
/// if we get too close wrap us around
|
|
/// CHANGE THIS to navigate away from whatever it is we are too close to
|
|
/// </summary>
|
|
void AvoidBorders ()
|
|
{
|
|
if (m_loc.X < 5 || m_loc.X > 250)
|
|
m_vel.X = -m_vel.X;
|
|
if (m_loc.Y < 5 || m_loc.Y > 250)
|
|
m_vel.Y = -m_vel.Y;
|
|
if (m_loc.Z < 21 || m_loc.Z > 271 )
|
|
m_vel.Z = -m_vel.Z;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Separate ourselves from the specified boids.
|
|
/// keeps us a respectable distance from our closest neighbours whilst still
|
|
/// being part of our local flock
|
|
/// </summary>
|
|
/// <param name='boids'>
|
|
/// Boids. all the boids in the scene
|
|
/// </param>
|
|
Vector3 Separate (List<Boid> boids)
|
|
{
|
|
Vector3 steer = new Vector3 (0, 0, 0);
|
|
int count = 0;
|
|
// For every boid in the system, check if it's too close
|
|
foreach (Boid other in boids) {
|
|
float d = Vector3.Distance (m_loc, other.Location);
|
|
// If the distance is greater than 0 and less than an arbitrary amount (0 when you are yourself)
|
|
if ((d > 0) && (d < m_desiredSeparation)) {
|
|
// Calculate vector pointing away from neighbor
|
|
Vector3 diff = Vector3.Subtract (m_loc, other.Location);
|
|
diff.Normalize ();
|
|
diff = Vector3.Divide (diff, d);
|
|
steer = Vector3.Add (steer, diff);
|
|
count++; // Keep track of how many
|
|
}
|
|
}
|
|
// Average -- divide by how many
|
|
if (count > 0) {
|
|
steer /= (float)count;
|
|
}
|
|
|
|
// As long as the vector is greater than 0
|
|
if (steer.Length () > 0) {
|
|
// Implement Reynolds: Steering = Desired - Velocity
|
|
steer.Normalize ();
|
|
steer *= m_maxSpeed;
|
|
steer -= m_vel;
|
|
//steer.limit(maxforce);
|
|
steer = Util.Limit (steer, m_maxForce);
|
|
}
|
|
return steer;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Align our boid within the flock.
|
|
/// For every nearby boid in the system, calculate the average velocity
|
|
/// and move us towards that - this keeps us moving with the flock.
|
|
/// </summary>
|
|
/// <param name='boids'>
|
|
/// Boids. all the boids in the scene - we only really care about those in the neighbourdist
|
|
/// </param>
|
|
Vector3 Align (List<Boid> boids)
|
|
{
|
|
Vector3 steer = new Vector3 (0, 0, 0);
|
|
int count = 0;
|
|
foreach (Boid other in boids) {
|
|
float d = Vector3.Distance (m_loc, other.Location);
|
|
if ((d > 0) && (d < m_neighborDist)) {
|
|
steer += other.Velocity;
|
|
count++;
|
|
}
|
|
}
|
|
if (count > 0) {
|
|
steer /= (float)count;
|
|
}
|
|
|
|
// As long as the vector is greater than 0
|
|
if (steer.Length () > 0) {
|
|
// Implement Reynolds: Steering = Desired - Velocity
|
|
steer.Normalize ();
|
|
steer *= m_maxSpeed;
|
|
steer -= m_vel;
|
|
//steer.limit(maxforce);
|
|
steer = Util.Limit (steer, m_maxForce);
|
|
|
|
}
|
|
return steer;
|
|
}
|
|
|
|
/// <summary>
|
|
/// MAintain the cohesion of our local flock
|
|
/// For the average location (i.e. center) of all nearby boids, calculate our steering vector towards that location
|
|
/// </summary>
|
|
/// <param name='boids'>
|
|
/// Boids. the boids in the scene
|
|
/// </param>
|
|
Vector3 Cohesion (List<Boid> boids)
|
|
{
|
|
|
|
Vector3 sum = Vector3.Zero; // Start with empty vector to accumulate all locations
|
|
int count = 0;
|
|
|
|
foreach (Boid other in boids) {
|
|
float d = Vector3.Distance (m_loc, other.Location);
|
|
if ((d > 0) && (d < m_neighborDist)) {
|
|
sum += other.Location; // Add location
|
|
count++;
|
|
}
|
|
}
|
|
if (count > 0) {
|
|
sum /= (float)count;
|
|
return Steer (sum, false); // Steer towards the location
|
|
}
|
|
return sum;
|
|
}
|
|
}
|
|
}
|
|
|