public void Step(bool momentum = true)
{
if (momentum)
{
foreach (Node node in Nodes)
{
node.Position += node.Velocity;
}
}
Parallel.ForEach(Goals, goal => goal.Compute(Nodes));
Triple[] nodeMoveSums = new Triple[Nodes.Count];
float[] nodeWeightSums = new float[Nodes.Count];
foreach (Goal goal in Goals)
{
for (int i = 0; i < goal.NodeCount; i++)
{
nodeMoveSums[goal.NodeIndices[i]] += goal.Moves[i] * goal.Weight;
nodeWeightSums[goal.NodeIndices[i]] += goal.Weight;
}
}
//=================================================================================
if (HandleNodeIndex != -1)
{
float mouseInteractionWeight = 30f;
nodeWeightSums[HandleNodeIndex] += mouseInteractionWeight;
Triple clickRayOrigin = new Triple(ClickRay.Origin.X, ClickRay.Origin.Y, ClickRay.Origin.Z);
Triple clickRayDirection = new Triple(ClickRay.Direction.X, ClickRay.Direction.Y, ClickRay.Direction.Z);
clickRayDirection = clickRayDirection.Normalise();
Triple v = Nodes[HandleNodeIndex].Position - clickRayOrigin;
Triple grabMove = v.Dot(clickRayDirection) * clickRayDirection - v;
nodeMoveSums[HandleNodeIndex] += mouseInteractionWeight * grabMove;
}
//=================================================================================
for (int i = 0; i < Nodes.Count; i++)
{
Triple move = nodeMoveSums[i] / nodeWeightSums[i];
Nodes[i].Position += move;
if (momentum)
{
Nodes[i].Velocity += move;
}
if (Nodes[i].Velocity.Dot(move) < 0.0)
{
Nodes[i].Velocity *= 0.9f;
}
}
}
public Triple Rotate(Triple origin, Triple Axis, float angle)
{
Triple z = Axis.Normalise();
Triple x = z.GeneratePerpendicular().Normalise();
Triple y = z.Cross(x).Normalise();
Triple v = this - origin;
float vx = x.Dot(v);
float vy = y.Dot(v);
float vz = z.Dot(v);
float sin = (float)Math.Sin(angle);
float cos = (float)Math.Cos(angle);
float vx_ = cos * vx - sin * vy;
float vy_ = sin * vx + cos * vy;
return(origin + x * vx_ + y * vy_ + z * vz);
}