public static void print_compare(string prefix, Frame3f v1, g3.Frame3f v2)
{
double dErr =
(v1.Origin - v2.Origin).Length +
((g3.Quaternionf)v1.Rotation - (v2.Rotation)).Length;
DebugUtil.Log(2, "{0} {1} {2} err {3}", prefix, v1.ToString("F8"), v2.ToString(), dErr);
}
internal MeshNode(int i, int fi, g3.Frame3f f, g3.Index3i neighbors_index, g3.Index3i vertex_index)
{
frame = f;
this.neighbors_index = neighbors_index;
this.vertex_index = vertex_index;
meshIndex = fi;
index = i;
}
/// <summary> Map mesh *from* local frame coordinates into "world" coordinates </summary>
public static void FromFrame(IDeformableMesh mesh, Frame3f f)
{
int NV = mesh.MaxVertexID;
bool bHasNormals = mesh.HasVertexNormals;
for (int vid = 0; vid < NV; ++vid)
{
if (mesh.IsVertex(vid))
{
Vector3d vf = mesh.GetVertex(vid);
Vector3d v = f.FromFrameP((Vector3f)vf);
mesh.SetVertex(vid, v);
if (bHasNormals)
{
Vector3f n = mesh.GetVertexNormal(vid);
Vector3f nf = f.FromFrameV(n);
mesh.SetVertexNormal(vid, nf);
}
}
}
}
// returns max edge length of moved vertices, after deformation
public override DeformInfo Apply(Frame3f vNextPos)
{
// if we did not move brush far enough, don't do anything
Vector3d vDelta = (vNextPos.Origin - vPreviousPos.Origin);
if (vDelta.Length < 0.0001f)
{
return(new DeformInfo()
{
bNoChange = true, maxEdgeLenSqr = 0, minEdgeLenSqr = double.MaxValue
});
}
// otherwise apply base deformation
DeformF = (idx, t) =>
{
Vector3d v = vPreviousPos.ToFrameP(Curve[idx]);
Vector3d vNew = vNextPos.FromFrameP(v);
return(Vector3d.Lerp(Curve[idx], vNew, t));
};
return(base.Apply(vNextPos));
}
/// <summary>
/// calculate extents of mesh along axes of frame, with optional transform
/// </summary>
public static AxisAlignedBox3d BoundsInFrame(DMesh3 mesh, Frame3f frame, Func <Vector3d, Vector3d> TransformF = null)
{
AxisAlignedBox3d bounds = AxisAlignedBox3d.Empty;
if (TransformF == null)
{
foreach (Vector3d v in mesh.Vertices())
{
Vector3d fv = frame.ToFrameP(v);
bounds.Contain(ref fv);
}
}
else
{
foreach (Vector3d v in mesh.Vertices())
{
Vector3d vT = TransformF(v);
Vector3d fv = frame.ToFrameP(ref vT);
bounds.Contain(ref fv);
}
}
return(bounds);
}
public virtual int[] AddTriangleStrip(IList <Frame3f> frames, IList <Interval1d> spans, int group_id = -1)
{
int N = frames.Count;
if (N != spans.Count)
{
throw new Exception("MeshEditor.AddTriangleStrip: spans list is not the same size!");
}
int[] new_tris = new int[2 * (N - 1)];
int prev_a = -1, prev_b = -1;
int i = 0, ti = 0;
for (i = 0; i < N; ++i)
{
Frame3f f = frames[i];
Interval1d span = spans[i];
Vector3d va = f.Origin + (float)span.a * f.Y;
Vector3d vb = f.Origin + (float)span.b * f.Y;
// [TODO] could compute normals here...
int a = Mesh.AppendVertex(va);
int b = Mesh.AppendVertex(vb);
if (prev_a != -1)
{
new_tris[ti++] = Mesh.AppendTriangle(prev_a, b, prev_b);
new_tris[ti++] = Mesh.AppendTriangle(prev_a, a, b);
}
prev_a = a; prev_b = b;
}
return(new_tris);
}
public static void AppendBox(DMesh3 mesh, Frame3f frame, Vector3f size, Colorf color)
{
var editor = new MeshEditor(mesh);
editor.AppendBox(frame, size, color);
}
public FrameGridIndexer3(Frame3f frame, Vector3f cellSize)
{
GridFrame = frame;
CellSize = cellSize;
}
public static void Store(Frame3f vFrame, BinaryWriter writer)
{
Store(vFrame.Origin, writer);
Store(vFrame.Rotation, writer);
}
override public MeshGenerator Generate()
{
if (Polygon == null)
{
Polygon = Polygon2d.MakeCircle(1.0f, 8);
}
int NV = Vertices.Count;
int Slices = Polygon.VertexCount;
int nRings = (ClosedLoop && NoSharedVertices) ? NV + 1 : NV;
int nRingSize = (NoSharedVertices) ? Slices + 1 : Slices;
int nCapVertices = (NoSharedVertices) ? Slices + 1 : 1;
if (Capped == false || ClosedLoop == true)
{
nCapVertices = 0;
}
vertices = new VectorArray3d(nRings * nRingSize + 2 * nCapVertices);
uv = new VectorArray2f(vertices.Count);
normals = new VectorArray3f(vertices.Count);
int quad_strips = (ClosedLoop) ? NV : NV - 1;
int nSpanTris = quad_strips * (2 * Slices);
int nCapTris = (Capped && ClosedLoop == false) ? 2 * Slices : 0;
triangles = new IndexArray3i(nSpanTris + nCapTris);
Frame3f fCur = new Frame3f(Frame);
Vector3d dv = CurveUtils.GetTangent(Vertices, 0, ClosedLoop);
fCur.Origin = (Vector3f)Vertices[0];
fCur.AlignAxis(2, (Vector3f)dv);
Frame3f fStart = new Frame3f(fCur);
double circumference = Polygon.ArcLength;
double pathLength = CurveUtils.ArcLength(Vertices, ClosedLoop);
double accum_path_u = 0;
// generate tube
for (int ri = 0; ri < nRings; ++ri)
{
int vi = ri % NV;
// propagate frame
Vector3d tangent = CurveUtils.GetTangent(Vertices, vi, ClosedLoop);
fCur.Origin = (Vector3f)Vertices[vi];
fCur.AlignAxis(2, (Vector3f)tangent);
// generate vertices
int nStartR = ri * nRingSize;
double accum_ring_v = 0;
for (int j = 0; j < nRingSize; ++j)
{
int k = nStartR + j;
Vector2d pv = Polygon.Vertices[j % Slices];
Vector2d pvNext = Polygon.Vertices[(j + 1) % Slices];
Vector3d v = fCur.FromPlaneUV((Vector2f)pv, 2);
vertices[k] = v;
uv[k] = new Vector2f(accum_path_u, accum_ring_v);
accum_ring_v += (pv.Distance(pvNext) / circumference);
Vector3f n = (Vector3f)(v - fCur.Origin).Normalized;
normals[k] = n;
}
int viNext = (ri + 1) % NV;
double d = Vertices[vi].Distance(Vertices[viNext]);
accum_path_u += d / pathLength;
}
// generate triangles
int ti = 0;
int nStop = (ClosedLoop && NoSharedVertices == false) ? nRings : (nRings - 1);
for (int ri = 0; ri < nStop; ++ri)
{
int r0 = ri * nRingSize;
int r1 = r0 + nRingSize;
if (ClosedLoop && ri == nStop - 1 && NoSharedVertices == false)
{
r1 = 0;
}
for (int k = 0; k < nRingSize - 1; ++k)
{
triangles.Set(ti++, r0 + k, r0 + k + 1, r1 + k + 1, Clockwise);
triangles.Set(ti++, r0 + k, r1 + k + 1, r1 + k, Clockwise);
}
if (NoSharedVertices == false) // last quad if we aren't sharing vertices
{
int M = nRingSize - 1;
triangles.Set(ti++, r0 + M, r0, r1, Clockwise);
triangles.Set(ti++, r0 + M, r1, r1 + M, Clockwise);
}
}
if (Capped && ClosedLoop == false)
{
Vector2d c = (OverrideCapCenter) ? CapCenter : Polygon.Bounds.Center;
// add endcap verts
int nBottomC = nRings * nRingSize;
vertices[nBottomC] = fStart.FromPlaneUV((Vector2f)c, 2);
uv[nBottomC] = new Vector2f(0.5f, 0.5f);
normals[nBottomC] = -fStart.Z;
startCapCenterIndex = nBottomC;
int nTopC = nBottomC + 1;
vertices[nTopC] = fCur.FromPlaneUV((Vector2f)c, 2);
uv[nTopC] = new Vector2f(0.5f, 0.5f);
normals[nTopC] = fCur.Z;
endCapCenterIndex = nTopC;
if (NoSharedVertices)
{
// duplicate first loop and make a fan w/ bottom-center
int nExistingB = 0;
int nStartB = nTopC + 1;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartB + k] = vertices[nExistingB + k];
Vector2d vuv = ((Polygon[k] - c).Normalized + Vector2d.One) * 0.5;
uv[nStartB + k] = (Vector2f)vuv;
normals[nStartB + k] = normals[nBottomC];
}
append_disc(Slices, nBottomC, nStartB, true, Clockwise, ref ti);
// duplicate second loop and make fan
int nExistingT = nRingSize * (nRings - 1);
int nStartT = nStartB + Slices;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartT + k] = vertices[nExistingT + k];
uv[nStartT + k] = uv[nStartB + k];
normals[nStartT + k] = normals[nTopC];
}
append_disc(Slices, nTopC, nStartT, true, !Clockwise, ref ti);
}
else
{
append_disc(Slices, nBottomC, 0, true, Clockwise, ref ti);
append_disc(Slices, nTopC, nRingSize * (nRings - 1), true, !Clockwise, ref ti);
}
}
return(this);
}
/// <summary>
/// Find point-normal frame at ray-intersection point on mesh, or return false if no hit.
/// Returns interpolated vertex-normal frame if available, otherwise tri-normal frame.
/// </summary>
public static bool RayHitPointFrame(DMesh3 mesh, ISpatial spatial, Ray3d ray, out Frame3f hitPosFrame, bool bForceFaceNormal = false)
{
hitPosFrame = new Frame3f();
int tid = spatial.FindNearestHitTriangle(ray);
if (tid == DMesh3.InvalidID)
{
return(false);
}
var isect = TriangleIntersection(mesh, tid, ray);
if (isect.Result != IntersectionResult.Intersects)
{
return(false);
}
Vector3d surfPt = ray.PointAt(isect.RayParameter);
if (mesh.HasVertexNormals && bForceFaceNormal == false)
{
hitPosFrame = SurfaceFrame(mesh, tid, surfPt); // TODO isect has bary-coords already!!
}
else
{
hitPosFrame = new Frame3f(surfPt, mesh.GetTriNormal(tid));
}
return(true);
}
/// <summary>
/// Interpolate between two frames - Lerp for origin, Slerp for rotation
/// </summary>
public static Frame3f Interpolate(Frame3f f1, Frame3f f2, float t)
{
return(new Frame3f(
Vector3.Lerp(f1.origin, f2.origin, t),
Quaternion.Slerp(f1.rotation, f2.rotation, t)));
}
public void AppendBox(Frame3f frame, float size)
{
AppendBox(frame, size * Vector3f.One);
}
///<summary> Map frame *into* local coordinates of Frame </summary>
public Frame3f ToFrame(Frame3f f)
{
return(new Frame3f(ToFrameP(f.origin), ToFrame(f.rotation)));
}
public static Frame3f Interpolate(Frame3f f1, Frame3f f2, float alpha)
{
return(new Frame3f(
Vector3f.Lerp(f1.origin, f2.origin, alpha),
Quaternionf.Slerp(f1.rotation, f2.rotation, alpha)));
}
public static Frame3f Rotate(Frame3f f, Vector3d origin, Quaternionf rotation)
{
f.Rotate(rotation);
f.Origin = (Vector3f)Rotate(f.Origin, origin, rotation);
return(f);
}
public override void Generate()
{
int nRings = Curve.Length;
int nRingSize = (NoSharedVertices) ? Slices + 1 : Slices;
int nCapVertices = (NoSharedVertices) ? Slices + 1 : 1;
if (Capped == false)
{
nCapVertices = 0;
}
vertices = new VectorArray3d(nRingSize * nRings + 2 * nCapVertices);
uv = new VectorArray2f(vertices.Count);
normals = new VectorArray3f(vertices.Count);
int nSpanTris = (nRings - 1) * (2 * Slices);
int nCapTris = (Capped) ? 2 * Slices : 0;
triangles = new IndexArray3i(nSpanTris + nCapTris);
float fDelta = (float)((Math.PI * 2.0) / Slices);
Frame3f f = Axis;
// generate tube
for (int ri = 0; ri < nRings; ++ri)
{
Vector3d v_along = Curve[ri];
Vector3f v_frame = f.ToFrameP((Vector3f)v_along);
float uv_along = (float)ri / (float)(nRings - 1);
// generate vertices
int nStartR = ri * nRingSize;
for (int j = 0; j < nRingSize; ++j)
{
float angle = (float)j * fDelta;
// [TODO] this is not efficient...use Matrix3f?
Vector3f v_rot = Quaternionf.AxisAngleR(Vector3f.AxisY, angle) * v_frame;
Vector3d v_new = f.FromFrameP(v_rot);
int k = nStartR + j;
vertices[k] = v_new;
float uv_around = (float)j / (float)(nRingSize);
uv[k] = new Vector2f(uv_along, uv_around);
// [TODO] proper normal
Vector3f n = (Vector3f)(v_new - f.Origin).Normalized;
normals[k] = n;
}
}
// generate triangles
int ti = 0;
for (int ri = 0; ri < nRings - 1; ++ri)
{
int r0 = ri * nRingSize;
int r1 = r0 + nRingSize;
for (int k = 0; k < nRingSize - 1; ++k)
{
triangles.Set(ti++, r0 + k, r0 + k + 1, r1 + k + 1, Clockwise);
triangles.Set(ti++, r0 + k, r1 + k + 1, r1 + k, Clockwise);
}
if (NoSharedVertices == false) // close disc if we went all the way
{
triangles.Set(ti++, r1 - 1, r0, r1, Clockwise);
triangles.Set(ti++, r1 - 1, r1, r1 + nRingSize - 1, Clockwise);
}
}
if (Capped)
{
// find avg start loop size
Vector3d vAvgStart = Vector3d.Zero, vAvgEnd = Vector3d.Zero;
for (int k = 0; k < Slices; ++k)
{
vAvgStart += vertices[k];
vAvgEnd += vertices[(nRings - 1) * nRingSize + k];
}
vAvgStart /= (double)Slices;
vAvgEnd /= (double)Slices;
Frame3f fStart = f;
fStart.Origin = (Vector3f)vAvgStart;
Frame3f fEnd = f;
fEnd.Origin = (Vector3f)vAvgEnd;
// add endcap verts
int nBottomC = nRings * nRingSize;
vertices[nBottomC] = fStart.Origin;
uv[nBottomC] = new Vector2f(0.5f, 0.5f);
normals[nBottomC] = -fStart.Z;
startCapCenterIndex = nBottomC;
int nTopC = nBottomC + 1;
vertices[nTopC] = fEnd.Origin;
uv[nTopC] = new Vector2f(0.5f, 0.5f);
normals[nTopC] = fEnd.Z;
endCapCenterIndex = nTopC;
if (NoSharedVertices)
{
// duplicate first loop and make a fan w/ bottom-center
int nExistingB = 0;
int nStartB = nTopC + 1;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartB + k] = vertices[nExistingB + k];
//uv[nStartB + k] = (Vector2f)Polygon.Vertices[k].Normalized;
float angle = (float)k * fDelta;
double cosa = Math.Cos(angle), sina = Math.Sin(angle);
uv[nStartB + k] = new Vector2f(0.5f * (1.0f + cosa), 0.5f * (1 + sina));
normals[nStartB + k] = normals[nBottomC];
}
append_disc(Slices, nBottomC, nStartB, true, Clockwise, ref ti);
// duplicate second loop and make fan
int nExistingT = nRingSize * (nRings - 1);
int nStartT = nStartB + Slices;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartT + k] = vertices[nExistingT + k];
//uv[nStartT + k] = (Vector2f)Polygon.Vertices[k].Normalized;
float angle = (float)k * fDelta;
double cosa = Math.Cos(angle), sina = Math.Sin(angle);
uv[nStartT + k] = new Vector2f(0.5f * (1.0f + cosa), 0.5f * (1 + sina));
normals[nStartT + k] = normals[nTopC];
}
append_disc(Slices, nTopC, nStartT, true, !Clockwise, ref ti);
}
else
{
append_disc(Slices, nBottomC, 0, true, Clockwise, ref ti);
append_disc(Slices, nTopC, nRingSize * (nRings - 1), true, !Clockwise, ref ti);
}
}
}
public void AppendBox(Frame3f frame, Vector3f size)
{
AppendBox(frame, size, Colorf.White);
}
public override DeformInfo Apply(Frame3f vNextPos)
{
Interval1d edgeRangeSqr = Interval1d.Empty;
int N = Curve.VertexCount;
if (N > NewV.size)
{
NewV.resize(N);
}
if (N > ModifiedV.Length)
{
ModifiedV = new BitArray(2 * N);
}
// clear modified
ModifiedV.SetAll(false);
bool bSmooth = (SmoothAlpha > 0 && SmoothIterations > 0);
double r2 = Radius * Radius;
// deform pass
if (DeformF != null)
{
for (int i = 0; i < N; ++i)
{
Vector3d v = Curve[i];
double d2 = (v - vPreviousPos.Origin).LengthSquared;
if (d2 < r2)
{
double t = WeightFunc(Math.Sqrt(d2), Radius);
Vector3d vNew = DeformF(i, t);
if (bSmooth == false)
{
if (i > 0)
{
edgeRangeSqr.Contain(vNew.DistanceSquared(Curve[i - 1]));
}
if (i < N - 1)
{
edgeRangeSqr.Contain(vNew.DistanceSquared(Curve[i + 1]));
}
}
NewV[i] = vNew;
ModifiedV[i] = true;
}
}
}
else
{
// anything?
}
// smooth pass
if (bSmooth)
{
for (int j = 0; j < SmoothIterations; ++j)
{
int iStart = (Curve.Closed) ? 0 : 1;
int iEnd = (Curve.Closed) ? N : N - 1;
for (int i = iStart; i < iEnd; ++i)
{
Vector3d v = (ModifiedV[i]) ? NewV[i] : Curve[i];
double d2 = (v - vPreviousPos.Origin).LengthSquared;
if (ModifiedV[i] || d2 < r2) // always smooth any modified verts
{
double a = SmoothAlpha * WeightFunc(Math.Sqrt(d2), Radius);
int iPrev = (i == 0) ? N - 1 : i - 1;
int iNext = (i + 1) % N;
Vector3d vPrev = (ModifiedV[iPrev]) ? NewV[iPrev] : Curve[iPrev];
Vector3d vNext = (ModifiedV[iNext]) ? NewV[iNext] : Curve[iNext];
Vector3d c = (vPrev + vNext) * 0.5f;
NewV[i] = (1 - a) * v + (a) * c;
ModifiedV[i] = true;
if (i > 0)
{
edgeRangeSqr.Contain(NewV[i].DistanceSquared(Curve[i - 1]));
}
if (i < N - 1)
{
edgeRangeSqr.Contain(NewV[i].DistanceSquared(Curve[i + 1]));
}
}
}
}
}
// bake
for (int i = 0; i < N; ++i)
{
if (ModifiedV[i])
{
Curve[i] = NewV[i];
}
}
return(new DeformInfo()
{
minEdgeLenSqr = edgeRangeSqr.a, maxEdgeLenSqr = edgeRangeSqr.b
});
}
public Cylinder3d(Frame3f frame, double radius, double height, int nNormalAxis = 1)
{
Axis = new Line3d(frame.Origin, frame.GetAxis(nNormalAxis));
Radius = radius;
Height = height;
}
public abstract DeformInfo Apply(Frame3f vNextPos);
public virtual void BeginDeformation(Frame3f vStartPos)
{
vPreviousPos = vStartPos;
}
override public void Generate()
{
if (Polygon == null)
{
Polygon = Polygon2d.MakeCircle(1.0f, 8);
}
int Slices = Polygon.VertexCount;
int nRings = Vertices.Count;
int nRingSize = (NoSharedVertices) ? Slices + 1 : Slices;
int nCapVertices = (NoSharedVertices) ? Slices + 1 : 1;
if (Capped == false)
{
nCapVertices = 0;
}
vertices = new VectorArray3d(nRings * nRingSize + 2 * nCapVertices);
uv = new VectorArray2f(vertices.Count);
normals = new VectorArray3f(vertices.Count);
int nSpanTris = (Vertices.Count - 1) * (2 * Slices);
int nCapTris = (Capped) ? 2 * Slices : 0;
triangles = new IndexArray3i(nSpanTris + nCapTris);
Frame3f fCur = new Frame3f(Frame);
Vector3d dv = CurveUtils.GetTangent(Vertices, 0);;
fCur.Origin = (Vector3f)Vertices[0];
fCur.AlignAxis(2, (Vector3f)dv);
Frame3f fStart = new Frame3f(fCur);
// generate tube
for (int ri = 0; ri < nRings; ++ri)
{
// propagate frame
if (ri != 0)
{
Vector3d tan = CurveUtils.GetTangent(Vertices, ri);
fCur.Origin = (Vector3f)Vertices[ri];
if (ri == 11)
{
dv = tan;
}
fCur.AlignAxis(2, (Vector3f)tan);
}
float uv_along = (float)ri / (float)(nRings - 1);
// generate vertices
int nStartR = ri * nRingSize;
for (int j = 0; j < nRingSize; ++j)
{
float uv_around = (float)j / (float)(nRings);
int k = nStartR + j;
Vector2d pv = Polygon.Vertices[j % Slices];
Vector3d v = fCur.FromFrameP((Vector2f)pv, 2);
vertices[k] = v;
uv[k] = new Vector2f(uv_along, uv_around);
Vector3f n = (Vector3f)(v - fCur.Origin).Normalized;
normals[k] = n;
}
}
// generate triangles
int ti = 0;
for (int ri = 0; ri < nRings - 1; ++ri)
{
int r0 = ri * nRingSize;
int r1 = r0 + nRingSize;
for (int k = 0; k < nRingSize - 1; ++k)
{
triangles.Set(ti++, r0 + k, r0 + k + 1, r1 + k + 1, Clockwise);
triangles.Set(ti++, r0 + k, r1 + k + 1, r1 + k, Clockwise);
}
if (NoSharedVertices == false) // close disc if we went all the way
{
triangles.Set(ti++, r1 - 1, r0, r1, Clockwise);
triangles.Set(ti++, r1 - 1, r1, r1 + nRingSize - 1, Clockwise);
}
}
if (Capped)
{
// add endcap verts
int nBottomC = nRings * nRingSize;
vertices[nBottomC] = fStart.Origin;
uv[nBottomC] = new Vector2f(0.5f, 0.5f);
normals[nBottomC] = -fStart.Z;
startCapCenterIndex = nBottomC;
int nTopC = nBottomC + 1;
vertices[nTopC] = fCur.Origin;
uv[nTopC] = new Vector2f(0.5f, 0.5f);
normals[nTopC] = fCur.Z;
endCapCenterIndex = nTopC;
if (NoSharedVertices)
{
// duplicate first loop and make a fan w/ bottom-center
int nExistingB = 0;
int nStartB = nTopC + 1;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartB + k] = vertices[nExistingB + k];
uv[nStartB + k] = (Vector2f)Polygon.Vertices[k].Normalized;
normals[nStartB + k] = normals[nBottomC];
}
append_disc(Slices, nBottomC, nStartB, true, Clockwise, ref ti);
// duplicate second loop and make fan
int nExistingT = nRingSize * (nRings - 1);
int nStartT = nStartB + Slices;
for (int k = 0; k < Slices; ++k)
{
vertices[nStartT + k] = vertices[nExistingT + k];
uv[nStartT + k] = (Vector2f)Polygon.Vertices[k].Normalized;
normals[nStartT + k] = normals[nTopC];
}
append_disc(Slices, nTopC, nStartT, true, !Clockwise, ref ti);
}
else
{
append_disc(Slices, nBottomC, 0, true, Clockwise, ref ti);
append_disc(Slices, nTopC, nRingSize * (nRings - 1), true, !Clockwise, ref ti);
}
}
}
///<summary> Map frame *into* local coordinates of Frame </summary>
public Frame3f ToFrame(ref Frame3f f)
{
return new Frame3f(ToFrameP(ref f.origin), ToFrame(ref f.rotation));
}
public Frame3f(Frame3f copy)
{
this.rotation = copy.rotation;
this.origin = copy.origin;
}
/// <summary> Map frame *from* local frame coordinates into "world" coordinates </summary>
public Frame3f FromFrame(ref Frame3f f)
{
return new Frame3f(FromFrameP(ref f.origin), FromFrame(ref f.rotation));
}
/// <summary> Map frame *from* local frame coordinates into "world" coordinates </summary>
public Frame3f FromFrame(Frame3f f)
{
return(new Frame3f(FromFrameP(f.origin), FromFrame(f.rotation)));
}
public bool PrecisionEqual(Frame3f f2, int nDigits)
{
return(origin.PrecisionEqual(f2.origin, nDigits) &&
rotation.PrecisionEqual(f2.rotation, nDigits));
}
public bool EpsilonEqual(Frame3f f2, float epsilon)
{
return(origin.EpsilonEqual(f2.origin, epsilon) &&
rotation.EpsilonEqual(f2.rotation, epsilon));
}
void constrained_smooth(DGraph3 graph, double surfDist, double dotThresh, double alpha, int rounds)
{
int NV = graph.MaxVertexID;
Vector3d[] pos = new Vector3d[NV];
for (int ri = 0; ri < rounds; ++ri) {
gParallel.ForEach(graph.VertexIndices(), (vid) => {
Vector3d v = graph.GetVertex(vid);
if ( GroundVertices.Contains(vid) || TipVertices.Contains(vid) ) {
pos[vid] = v;
return;
}
// for tip base vertices, we could allow them to move down and away within angle cone...
if (TipBaseVertices.Contains(vid)) {
pos[vid] = v;
return;
}
// compute smoothed position of vtx
Vector3d centroid = Vector3d.Zero; int nbr_count = 0;
foreach (int nbr_vid in graph.VtxVerticesItr(vid)) {
centroid += graph.GetVertex(nbr_vid);
nbr_count++;
}
if (nbr_count == 1) {
pos[vid] = v;
return;
}
centroid /= nbr_count;
Vector3d vnew = (1 - alpha) * v + (alpha) * centroid;
// make sure we don't violate angle constraint to any nbrs
int attempt = 0;
try_again:
foreach ( int nbr_vid in graph.VtxVerticesItr(vid)) {
Vector3d dv = graph.GetVertex(nbr_vid) - vnew;
dv.Normalize();
double dot = dv.Dot(Vector3d.AxisY);
if ( Math.Abs(dot) < dotThresh ) {
if (attempt++ < 3) {
vnew = Vector3d.Lerp(v, vnew, 0.66);
goto try_again;
} else {
pos[vid] = v;
return;
}
}
}
// offset from nearest point on surface
Frame3f fNearest = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, vnew, true);
Vector3d vNearest = fNearest.Origin;
double dist = vnew.Distance(vNearest);
bool inside = MeshSpatial.IsInside(vnew);
if (inside || dist < surfDist) {
Vector3d normal = fNearest.Z;
// don't push down?
if (normal.Dot(Vector3d.AxisY) < 0) {
normal.y = 0; normal.Normalize();
}
vnew = fNearest.Origin + surfDist * normal;
}
pos[vid] = vnew;
});
foreach (int vid in graph.VertexIndices())
graph.SetVertex(vid, pos[vid]);
}
}