internal int FindNearestNodeIndex(float range = 0.03f)
{
CameraData cameraData = DynaShapeViewExtension.CameraData;
Triple camZ = new Triple(
cameraData.LookDirection.X,
-cameraData.LookDirection.Z,
cameraData.LookDirection.Y).Normalise();
Triple camY = new Triple(
cameraData.UpDirection.X,
-cameraData.UpDirection.Z,
cameraData.UpDirection.Y).Normalise();
Triple camX = camY.Cross(camZ).Normalise();
Triple mousePosition2D = new Triple(
DynaShapeViewExtension.MouseRayDirection.Dot(camX),
DynaShapeViewExtension.MouseRayDirection.Dot(camY),
DynaShapeViewExtension.MouseRayDirection.Dot(camZ));
mousePosition2D /= mousePosition2D.Z;
int nearestNodeIndex = -1;
float minDistSquared = range * range;
for (int i = 0; i < Nodes.Count; i++)
{
Triple v = Nodes[i].Position - DynaShapeViewExtension.MouseRayOrigin;
v = new Triple(v.Dot(camX), v.Dot(camY), v.Dot(camZ));
Triple nodePosition2D = v / v.Z;
float distSquared = (mousePosition2D - nodePosition2D).LengthSquared;
if (distSquared < minDistSquared)
{
minDistSquared = distSquared;
nearestNodeIndex = i;
}
}
return(nearestNodeIndex);
}
private void RenderGUI()
{
//============================================================================
// Render a visual marker to highlight the node that is being manipulated
//============================================================================
if (solver.HandleNodeIndex != -1 || solver.NearestNodeIndex != -1)
{
CameraData camera = DynaShapeViewExtension.CameraData;
Triple camOrigin = new Triple(camera.EyePosition.X, -camera.EyePosition.Z, camera.EyePosition.Y);
Triple camZ = new Triple(camera.LookDirection.X, -camera.LookDirection.Z, camera.LookDirection.Y).Normalise();
Triple camY = new Triple(camera.UpDirection.X, -camera.UpDirection.Z, camera.UpDirection.Y).Normalise();
Triple camX = camZ.Cross(camY);
int nodeIndex = solver.HandleNodeIndex != -1 ? solver.HandleNodeIndex : solver.NearestNodeIndex;
Triple v = solver.Nodes[nodeIndex].Position - camOrigin;
float screenDistance = (float)camera.NearPlaneDistance + 0.1f;
v = camOrigin + v * screenDistance / v.Dot(camZ);
float markerSize = 0.025f * screenDistance;
Triple v1 = v + camX * markerSize;
Triple v2 = v - camY * markerSize;
Triple v3 = v - camX * markerSize;
Triple v4 = v + camY * markerSize;
lineGeometry.Positions.Add(v1.ToVector3());
lineGeometry.Positions.Add(v3.ToVector3());
lineGeometry.Positions.Add(v2.ToVector3());
lineGeometry.Positions.Add(v4.ToVector3());
markerSize *= 0.5f;
v1 = v + camX * markerSize;
v2 = v - camY * markerSize;
v3 = v - camX * markerSize;
v4 = v + camY * markerSize;
lineGeometry.Positions.Add(v1.ToVector3());
lineGeometry.Positions.Add(v2.ToVector3());
lineGeometry.Positions.Add(v2.ToVector3());
lineGeometry.Positions.Add(v3.ToVector3());
lineGeometry.Positions.Add(v3.ToVector3());
lineGeometry.Positions.Add(v4.ToVector3());
lineGeometry.Positions.Add(v4.ToVector3());
lineGeometry.Positions.Add(v1.ToVector3());
int temp = lineGeometry.Indices.Count;
for (int i = 0; i < 12; i++)
{
lineGeometry.Indices.Add(temp + i);
lineGeometry.Colors.Add(Color.OrangeRed);
}
}
}
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 void Tessellate(IRenderPackage package, TessellationParameters parameters)
{
for (int i = 0; i < solver.Nodes.Count; i++)
{
GeometryRender.DrawPoint(package, solver.Nodes[i].Position);
}
for (int i = 0; i < solver.GeometryBinders.Count; i++)
{
solver.GeometryBinders[i].DrawGraphics(package, parameters, solver.Nodes);
}
//=======================================================================================
// Draw a cursor to highlight the node that is being manipulated by the mouse
// ... the curor is 2D, so we need to draw it in the camera coordinate system, manually
//=======================================================================================
CameraData camera = ((HelixWatch3DViewModel)Solver.Viewport).Camera.Dispatcher.Invoke(() => Solver.Viewport.GetCameraInformation());
Triple camOrigin = new Triple(camera.EyePosition.X, -camera.EyePosition.Z, camera.EyePosition.Y);
Triple camZ = new Triple(camera.LookDirection.X, -camera.LookDirection.Z, camera.LookDirection.Y).Normalise();
Triple camY = new Triple(camera.UpDirection.X, -camera.UpDirection.Z, camera.UpDirection.Y).Normalise();
Triple camX = camZ.Cross(camY);
if (solver.HandleNodeIndex != -1 || solver.NearestNodeIndex != -1)
{
int i = solver.HandleNodeIndex != -1 ? solver.HandleNodeIndex : solver.NearestNodeIndex;
Color handleColor = solver.HandleNodeIndex != -1 ? Color.OrangeRed : Color.Black;
Triple v = solver.Nodes[i].Position - camOrigin;
v = camOrigin + ((float)camera.NearPlaneDistance + 0.5f) * v / v.Dot(camZ);
float handleSize = 0.008f;
Triple v1 = v + camX * handleSize;
Triple v2 = v - camY * handleSize;
Triple v3 = v - camX * handleSize;
Triple v4 = v + camY * handleSize;
GeometryRender.DrawPolyline(package, new List <Triple> {
v1, v2, v3, v4, v1
}, handleColor);
handleSize = 0.015f;
v1 = v + camX * handleSize;
v2 = v - camY * handleSize;
v3 = v - camX * handleSize;
v4 = v + camY * handleSize;
GeometryRender.DrawLine(package, v1, v3, handleColor);
GeometryRender.DrawLine(package, v2, v4, handleColor);
}
}
internal int FindNearestNodeIndex(IRay clickRay, float range = 0.03f)
{
CameraData cameraData = Viewport.GetCameraInformation();
Triple camZ = new Triple(cameraData.LookDirection.X, -cameraData.LookDirection.Z,
cameraData.LookDirection.Y).Normalise();
Triple camY = new Triple(cameraData.UpDirection.X, -cameraData.UpDirection.Z, cameraData.UpDirection.Y)
.Normalise();
Triple camX = camY.Cross(camZ).Normalise();
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);
Triple mousePosition2D = new Triple(clickRayDirection.Dot(camX), clickRayDirection.Dot(camY),
clickRayDirection.Dot(camZ));
mousePosition2D /= mousePosition2D.Z;
int nearestNodeIndex = -1;
float minDistSquared = range * range;
for (int i = 0; i < Nodes.Count; i++)
{
Triple v = Nodes[i].Position - clickRayOrigin;
v = new Triple(v.Dot(camX), v.Dot(camY), v.Dot(camZ));
Triple nodePosition2D = v / v.Z;
float distSquared = (mousePosition2D - nodePosition2D).LengthSquared;
if (distSquared < minDistSquared)
{
minDistSquared = distSquared;
nearestNodeIndex = i;
}
}
return(nearestNodeIndex);
}
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);
}
public static bool ComputeBestFitCircle(List <Triple> points, out Triple circleCenter, out Triple circleNormal,
out float circleRadius)
{
// Apporach: Project the input points to the best fit plane, then fit a circle through these points using the the analytical approach ...
// ... described in the paper "A simple approach for the estimation of circular arc center and its radius" by Thomas S & Chan Y.
Triple planeOrigin, planeNormal;
int planeFittingResult = ComputeBestFitPlane(points, out planeOrigin, out planeNormal);
if (planeFittingResult < 2) // The points are either all identical, or colinear (i.e. already on the same "circle" of infinite radius)
{
circleCenter = circleNormal = new Triple(float.NaN);
circleRadius = float.NaN;
return(false);
}
Triple planeBasisX = planeNormal.GeneratePerpendicular().Normalise();
Triple planeBasisY = planeNormal.Cross(planeBasisX);
float sumX = 0f;
float sumY = 0f;
float sumX2 = 0f;
float sumY2 = 0f;
float sumXY = 0f;
float sumX3 = 0f;
float sumY3 = 0f;
float sumX2Y = 0f;
float sumXY2 = 0f;
float x, y;
for (int i = 0; i < points.Count; i++)
{
Triple p = points[i] - planeOrigin;
x = p.Dot(planeBasisX);
y = p.Dot(planeBasisY);
sumX += x;
sumY += y;
sumX2 += x * x;
sumY2 += y * y;
sumXY += x * y;
sumX3 += x * x * x;
sumY3 += y * y * y;
sumX2Y += x * x * y;
sumXY2 += x * y * y;
}
float n = points.Count;
float a1 = 2f * (sumX * sumX - n * sumX2);
float a2 = 2f * (sumX * sumY - n * sumXY);
float b1 = a2;
float b2 = 2f * (sumY * sumY - n * sumY2);
float c1 = sumX2 * sumX - n * sumX3 + sumX * sumY2 - n * sumXY2;
float c2 = sumX2 * sumY - n * sumY3 + sumY * sumY2 - n * sumX2Y;
float temp = 1f / (a1 * b2 - a2 * b1);
x = (c1 * b2 - c2 * b1) * temp;
y = (a1 * c2 - a2 * c1) * temp;
circleCenter = planeOrigin + (x * planeBasisX + y * planeBasisY);
circleRadius =
(float)Math.Sqrt((sumX2 - 2f * sumX * x + n * x * x + sumY2 - 2f * sumY * y + n * y * y) / n);
circleNormal = planeNormal;
return(true);
}
/// <summary>
/// Approximate the circle that best fit a set of input points
/// </summary>
/// <param name="points">The input points</param>
/// <param name="circleCenter">Center of the best fit circle</param>
/// <param name="circleNormal">Normal of the best fit circle</param>
/// <param name="circleRadius">Radius of the best fit circle</param>
/// <param name="tolerance">The tolerance that is used the determined if the input points are coincidental, colinear, or non-colinear</param>
/// <returns>False if the input points are coincidental or colinear; True otherwise</returns>
public static bool ComputeBestFitCircle(List <Triple> points, out Triple circleCenter, out Triple circleNormal, out float circleRadius, float tolerance = 1E-10f)
{
// The core idea is to project the input points to the best fit plane, then fit a circle through these points via an analytical approach.
// Reference: Thomas S & Chan Y: "A simple approach for the estimation of circular arc center and its radius
int planeFittingResult = ComputeBestFitPlane(points, out Triple planeOrigin, out Triple planeNormal, tolerance);
// Case 0: The input points are either coincidental or colinear
if (planeFittingResult < 2)
{
circleCenter = circleNormal = new Triple(float.NaN);
circleRadius = float.NaN;
return(false);
}
// Case 1: The input points are neither coincidental nor colinear
Triple planeBasisX = planeNormal.GeneratePerpendicular().Normalise();
Triple planeBasisY = planeNormal.Cross(planeBasisX);
float sumX = 0f;
float sumY = 0f;
float sumX2 = 0f;
float sumY2 = 0f;
float sumXY = 0f;
float sumX3 = 0f;
float sumY3 = 0f;
float sumX2Y = 0f;
float sumXY2 = 0f;
float x, y;
for (int i = 0; i < points.Count; i++)
{
Triple p = points[i] - planeOrigin;
x = p.Dot(planeBasisX);
y = p.Dot(planeBasisY);
sumX += x;
sumY += y;
sumX2 += x * x;
sumY2 += y * y;
sumXY += x * y;
sumX3 += x * x * x;
sumY3 += y * y * y;
sumX2Y += x * x * y;
sumXY2 += x * y * y;
}
float n = points.Count;
float a1 = 2f * (sumX * sumX - n * sumX2);
float a2 = 2f * (sumX * sumY - n * sumXY);
float b1 = a2;
float b2 = 2f * (sumY * sumY - n * sumY2);
float c1 = sumX2 * sumX - n * sumX3 + sumX * sumY2 - n * sumXY2;
float c2 = sumX2 * sumY - n * sumY3 + sumY * sumY2 - n * sumX2Y;
float temp = 1f / (a1 * b2 - a2 * b1);
x = (c1 * b2 - c2 * b1) * temp;
y = (a1 * c2 - a2 * c1) * temp;
circleCenter = planeOrigin + (x * planeBasisX + y * planeBasisY);
circleRadius = (float)Math.Sqrt((sumX2 - 2f * sumX * x + n * x * x + sumY2 - 2f * sumY * y + n * y * y) / n);
circleNormal = planeNormal;
return(true);
}
public void Iterate()
{
CurrentIteration++;
//=================================================================================
// Apply momentum
//=================================================================================
if (EnableMomentum)
{
foreach (Node node in Nodes)
{
node.Position += node.Velocity;
}
}
//=================================================================================
// Process each goal independently, in parallel
//=================================================================================
Parallel.ForEach(Goals, goal => goal.Compute(Nodes));
//=================================================================================
// Compute the total move vector that acts on each node
//=================================================================================
Triple[] nodeMoveSums = new Triple[Nodes.Count];
float[] nodeWeightSums = new float[Nodes.Count];
for (int j = 0; j < Goals.Count; j++)
{
Goal goal = Goals[j];
for (int i = 0; i < goal.NodeCount; i++)
{
nodeMoveSums[goal.NodeIndices[i]] += goal.Moves[i] * goal.Weights[i];
nodeWeightSums[goal.NodeIndices[i]] += goal.Weights[i];
}
}
//=================================================================================
// Move the manipulated node toward the mouse ray
//=================================================================================
#if CLI == false
if (HandleNodeIndex != -1)
{
float manipulationWeight = 30f;
nodeWeightSums[HandleNodeIndex] += manipulationWeight;
Triple v = Nodes[HandleNodeIndex].Position - DynaShapeViewExtension.MouseRayOrigin;
Triple mouseRayPull = v.Dot(DynaShapeViewExtension.MouseRayDirection) * DynaShapeViewExtension.MouseRayDirection - v;
nodeMoveSums[HandleNodeIndex] += manipulationWeight * mouseRayPull;
}
#endif
//=============================================================================================
// Move the nodes to their new positions
//=============================================================================================
if (EnableMomentum)
{
for (int i = 0; i < Nodes.Count; i++)
{
if (nodeWeightSums[i] == 0f)
{
continue;
}
Triple move = nodeMoveSums[i] / nodeWeightSums[i];
Nodes[i].Move = move;
Nodes[i].Position += move;
Nodes[i].Velocity += move;
//if (Nodes[i].Velocity.Dot(move) <= 0.0)
Nodes[i].Velocity *= DampingFactor;
}
}
else
{
for (int i = 0; i < Nodes.Count; i++)
{
if (nodeWeightSums[i] == 0f)
{
continue;
}
Triple move = nodeMoveSums[i] / nodeWeightSums[i];
Nodes[i].Move = move;
Nodes[i].Position += move;
Nodes[i].Velocity = Triple.Zero;
}
}
}
public void Iterate3()
{
CurrentIteration++;
//=================================================================================
// Apply momentum
//=================================================================================
foreach (Node node in Nodes)
{
node.Position += node.Velocity;
}
for (int k = 0; k < 50; k++)
{
//=================================================================================
// Process each goal indepently, in parallel
//=================================================================================
Parallel.ForEach(Goals, goal => goal.Compute(Nodes));
//=================================================================================
// Compute the total move vector that acts on each node
//=================================================================================
Triple[] nodeMoveSums = new Triple[Nodes.Count];
float[] nodeWeightSums = new float[Nodes.Count];
for (int j = 0; j < Goals.Count; j++)
{
Goal goal = Goals[j];
for (int i = 0; i < goal.NodeCount; i++)
{
nodeMoveSums[goal.NodeIndices[i]] += goal.Moves[i] * goal.Weight;
nodeWeightSums[goal.NodeIndices[i]] += goal.Weight;
}
}
//=================================================================================
// Move the manipulated node toward the mouse ray
//=================================================================================
if (HandleNodeIndex != -1)
{
float mouseInteractionWeight = 30f;
nodeWeightSums[HandleNodeIndex] += mouseInteractionWeight;
Triple v = Nodes[HandleNodeIndex].Position - DynaShapeViewExtension.MouseRayOrigin;
Triple mouseRayPull = v.Dot(DynaShapeViewExtension.MouseRayDirection) *
DynaShapeViewExtension.MouseRayDirection - v;
nodeMoveSums[HandleNodeIndex] += mouseInteractionWeight * mouseRayPull;
}
for (int i = 0; i < Nodes.Count; i++)
{
Triple move = nodeMoveSums[i] / nodeWeightSums[i];
Nodes[i].Position += move;
Nodes[i].Velocity += move;
if (Nodes[i].Velocity.Dot(move) < 0.0)
{
Nodes[i].Velocity *= 0.99f;
}
}
}
}
