// -----------------------------------------------------------------
/// <summary>
/// Compute the new velocity for one entity
/// </summary>
// -----------------------------------------------------------------
protected void UpdateVelocity(int start, NBodyValue valA, float timedelta)
{
int length = m_localIDList.Length;
for (int j = start + 1; j < length; j++)
{
UUID idB = m_localIDList[j];
if (idB == UUID.Zero)
{
continue;
}
NBodyValue valB = m_entityList[idB];
VelocityPair vp = ComputeVelocityDelta(valA, valB);
if (valA.CanMove && valB.CanAttract)
{
valA.Velocity += vp.ABVelocity * timedelta;
}
if (valB.CanMove && valA.CanAttract)
{
Vector3 nv = valB.Velocity + (vp.BAVelocity * timedelta);
valB.Velocity = nv;
}
}
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected override void HandleSimulationTaint()
{
m_log.WarnFormat("[N-BodyGroup] bump cosine similarity matrix to {0}", NextLocalID);
InitializeAttractionMatrix();
for (int i = 0; i < NextLocalID - 1; i++)
{
if (m_localIDList[i] == UUID.Zero)
{
continue;
}
NBodyValue v1 = m_entityList[m_localIDList[i]];
for (int j = i + 1; j < NextLocalID; j++)
{
if (m_localIDList[j] == UUID.Zero)
{
continue;
}
NBodyValue v2 = m_entityList[m_localIDList[j]];
double cosforce = ComputeCosineAttraction(v1, v2);
CachedAttraction[i][j] = (float)(UseMass ? cosforce * v2.Mass : cosforce);
CachedAttraction[j][i] = (float)(UseMass ? cosforce * v1.Mass : cosforce);
}
}
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
public void AddEntity(UUID entityID, double[] value, int canmove, int canattract, double mass, Vector3 velocity)
{
// make sure the object exists
SceneObjectPart sop = m_scene.GetSceneObjectPart(entityID);
if (sop == null)
{
m_log.WarnFormat("[N-BodyGroup] Cannot find SOP associated with {0}", entityID);
return;
}
lock (m_entityList)
{
NBodyValue nb;
if (!m_entityList.TryGetValue(entityID, out nb))
{
nb = new NBodyValue(entityID, NextLocalID++);
m_localIDList[nb.LocalID] = entityID;
m_entityList[entityID] = nb;
}
nb.Value = value;
nb.Mass = mass;
nb.Velocity = velocity;
nb.CanMove = (canmove != 0);
nb.CanAttract = (canattract != 0);
nb.Position = OSPosition2NBPosition(sop.AbsolutePosition);
Tainted = true;
}
m_log.DebugFormat("[N-BodyGroup] add entity {0}", entityID);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected override VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
UInt32 v1ID = v1.LocalID;
UInt32 v2ID = v2.LocalID;
Vector3 direction = Vector3.Normalize(v2.Position - v1.Position);
VelocityPair vp = new VelocityPair();
vp.ABVelocity = direction * CachedAttraction[(int)v1ID][(int)v2ID];
vp.BAVelocity = (-direction) * CachedAttraction[(int)v2ID][(int)v1ID];
return(vp);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
private double ComputeCosineAttraction(NBodyValue v1, NBodyValue v2)
{
double c = 0;
double vs1 = 0;
double vs2 = 0;
for (int i = 0; i < Dimension; i++)
{
vs1 += v1.Value[i] * v1.Value[i];
vs2 += v2.Value[i] * v2.Value[i];
c += v1.Value[i] * v2.Value[i];
}
// double force = c / Math.Sqrt(vs1 * vs2);
double force = (c * c) / (vs1 * vs2);
return(force - 0.5);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected override VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
VelocityPair vp = new VelocityPair();
Vector3 direction = Vector3.Normalize(v2.Position - v1.Position);
double dsquared = Vector3.DistanceSquared(v1.Position, v2.Position);
if (dsquared == 0)
{
dsquared = 0.0000000001;
}
// F = s * (m1 * m2) / d^2
// acceleration is the similarity attraction * mass of the attractor divided
// by the distance separating the body and attractor
double force = GravitationalConstant * v1.Mass * v2.Mass / dsquared;
// a = F/m
vp.ABVelocity = direction * (float)(force / v1.Mass);
vp.BAVelocity = (-direction) * (float)(force / v2.Mass);
return(vp);
}
// -----------------------------------------------------------------
/// <summary>
/// Compute the new position
/// This assumes velocity is in units/sec and timestep is proportion
/// of a second; also assumes that the position in the velocity was
/// updated "recently enough"
/// </summary>
// -----------------------------------------------------------------
protected void UpdatePosition(NBodyValue val, float timedelta)
{
if (!val.CanMove)
{
return;
}
SceneObjectPart sop = m_scene.GetSceneObjectPart(val.Identity);
if (sop == null)
{
return;
}
// If the object is selected, clear the velocity and move on...
if (sop.ParentGroup.IsSelected)
{
val.Velocity = Vector3.Zero;
return;
}
val.Position += val.Velocity * timedelta;
sop.UpdateGroupPosition(NBPosition2OSPosition(val.Position));
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
private double ComputeCosineAttraction(NBodyValue v1, NBodyValue v2)
{
double c = 0;
double vs1 = 0;
double vs2 = 0;
for (int i = 0; i < Dimension; i++)
{
vs1 += v1.Value[i] * v1.Value[i];
vs2 += v2.Value[i] * v2.Value[i];
c += v1.Value[i] * v2.Value[i];
}
// double force = c / Math.Sqrt(vs1 * vs2);
double force = (c * c) / (vs1 * vs2);
return force - 0.5;
}
// -----------------------------------------------------------------
/// <summary>
/// </summary>
// -----------------------------------------------------------------
protected override Vector3 ComputeInitialVelocity(NBodyValue v1)
{
return(UseVelocity ? v1.Velocity : Vector3.Zero);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected virtual Vector3 ComputeInitialVelocity(NBodyValue v1)
{
return(v1.Velocity);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected virtual VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
return(new VelocityPair());
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
public void AddEntity(UUID entityID, double[] value, int canmove, int canattract, double mass, Vector3 velocity)
{
// make sure the object exists
SceneObjectPart sop = m_scene.GetSceneObjectPart(entityID);
if (sop == null)
{
m_log.WarnFormat("[N-BodyGroup] Cannot find SOP associated with {0}",entityID);
return;
}
lock (m_entityList)
{
NBodyValue nb;
if (! m_entityList.TryGetValue(entityID,out nb))
{
nb = new NBodyValue(entityID,NextLocalID++);
m_localIDList[nb.LocalID] = entityID;
m_entityList[entityID] = nb;
}
nb.Value = value;
nb.Mass = mass;
nb.Velocity = velocity;
nb.CanMove = (canmove != 0);
nb.CanAttract = (canattract != 0);
nb.Position = OSPosition2NBPosition(sop.AbsolutePosition);
Tainted = true;
}
m_log.DebugFormat("[N-BodyGroup] add entity {0}",entityID);
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected override VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
UInt32 v1ID = v1.LocalID;
UInt32 v2ID = v2.LocalID;
Vector3 direction = Vector3.Normalize(v2.Position - v1.Position);
VelocityPair vp = new VelocityPair();
vp.ABVelocity = direction * CachedAttraction[(int)v1ID][(int)v2ID];
vp.BAVelocity = (-direction) * CachedAttraction[(int)v2ID][(int)v1ID];
return vp;
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected override VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
VelocityPair vp = new VelocityPair();
Vector3 direction = Vector3.Normalize(v2.Position-v1.Position);
double dsquared = Vector3.DistanceSquared(v1.Position,v2.Position);
if (dsquared == 0)
dsquared = 0.0000000001;
// F = s * (m1 * m2) / d^2
// acceleration is the similarity attraction * mass of the attractor divided
// by the distance separating the body and attractor
double force = GravitationalConstant * v1.Mass * v2.Mass / dsquared;
// a = F/m
vp.ABVelocity = direction * (float)(force / v1.Mass);
vp.BAVelocity = (-direction) * (float)(force / v2.Mass);
return vp;
}
// -----------------------------------------------------------------
/// <summary>
/// Compute the new velocity for one entity
/// </summary>
// -----------------------------------------------------------------
protected void UpdateVelocity(int start, NBodyValue valA, float timedelta)
{
int length = m_localIDList.Length;
for (int j = start + 1; j < length; j++)
{
UUID idB = m_localIDList[j];
if (idB == UUID.Zero)
continue;
NBodyValue valB = m_entityList[idB];
VelocityPair vp = ComputeVelocityDelta(valA,valB);
if (valA.CanMove && valB.CanAttract)
valA.Velocity += vp.ABVelocity * timedelta;
if (valB.CanMove && valA.CanAttract)
{
Vector3 nv = valB.Velocity + (vp.BAVelocity * timedelta);
valB.Velocity = nv;
}
}
}
// -----------------------------------------------------------------
/// <summary>
/// Compute the new position
/// This assumes velocity is in units/sec and timestep is proportion
/// of a second; also assumes that the position in the velocity was
/// updated "recently enough"
/// </summary>
// -----------------------------------------------------------------
protected void UpdatePosition(NBodyValue val, float timedelta)
{
if (! val.CanMove)
return;
SceneObjectPart sop = m_scene.GetSceneObjectPart(val.Identity);
if (sop == null)
return;
// If the object is selected, clear the velocity and move on...
if (sop.ParentGroup.IsSelected)
{
val.Velocity = Vector3.Zero;
return;
}
val.Position += val.Velocity * timedelta;
sop.UpdateGroupPosition(NBPosition2OSPosition(val.Position));
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected virtual VelocityPair ComputeVelocityDelta(NBodyValue v1, NBodyValue v2)
{
return new VelocityPair();
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
protected virtual Vector3 ComputeInitialVelocity(NBodyValue v1)
{
return v1.Velocity;
}
// -----------------------------------------------------------------
/// <summary>
///
/// </summary>
// -----------------------------------------------------------------
public Int32 Update(double timestep)
{
Int32 curtick = Util.EnvironmentTickCount();
float timedelta = (float)(timestep * TimeScale);
lock (m_entityList)
{
int length = m_localIDList.Length;
// Pass one... initialize all the velocities and update positions from the scene
for (int i = 0; i < length; i++)
{
UUID uuid = m_localIDList[i];
if (uuid == UUID.Zero)
{
continue;
}
SceneObjectPart sop = m_scene.GetSceneObjectPart(uuid);
if (sop == null)
{
m_entityList.Remove(uuid);
m_localIDList[i] = UUID.Zero;
continue;
}
NBodyValue value = m_entityList[uuid];
value.Position = OSPosition2NBPosition(sop.AbsolutePosition);
if (value.CanMove)
{
value.Velocity = ComputeInitialVelocity(value);
}
}
// Pass two... take care of an pre-computations that need to be done
if (Tainted)
{
HandleSimulationTaint();
Tainted = false;
}
// Pass three... compute the new velocities
// for (int i = 0; i < length; i++)
ParallelOptions opts = new ParallelOptions();
opts.MaxDegreeOfParallelism = SimulationThreads;
System.Threading.Tasks.Parallel.For(0, length, opts, i =>
{
UUID idA = m_localIDList[i];
if (idA != UUID.Zero)
{
NBodyValue valA = m_entityList[idA];
UpdateVelocity(i, valA, timedelta);
}
});
// Pass four... send out the updates
System.Threading.Tasks.Parallel.For(0, length, opts, i =>
{
UUID idA = m_localIDList[i];
if (idA != UUID.Zero)
{
NBodyValue valA = m_entityList[idA];
UpdatePosition(valA, timedelta);
}
});
}
return(Util.EnvironmentTickCountSubtract(curtick));
}
// -----------------------------------------------------------------
/// <summary>
/// </summary>
// -----------------------------------------------------------------
protected override Vector3 ComputeInitialVelocity(NBodyValue v1)
{
return UseVelocity ? v1.Velocity : Vector3.Zero;
}
