private void init()
{
center = Vector3d.xyz(0, 0, 0);
radius = 1;
numSlices = 16;
numStacks = 8;
}
public Edge(Vertex p1, Vertex p2)
{
this.p1 = p1;
this.p2 = p2;
direction = p2.pos.minus(p1.pos).normalized();
}
///
/// Constructor. Creates a sphere with the specified center, radius, number
/// of slices and stacks.
///
/// <param name="center">center of the sphere</param>
/// <param name="radius">sphere radius</param>
/// <param name="numSlices">number of slices</param>
/// <param name="numStacks">number of stacks</param>
///
public Sphere(IVector3d center, double radius, int numSlices, int numStacks)
{
this.center = center;
this.radius = radius;
this.numSlices = numSlices;
this.numStacks = numStacks;
}
/// <summary>
/// Returns the product of this matrix and the specified vector.
/// <b>Note:</b> the vector is not modified.
/// </summary>
///
/// <param name="a">the vector</param>
/// <returns>the product of this matrix and the specified vector</returns>
///
public IVector3d times(IVector3d a)
{
return(Vector3d.xyz(
m11 * a.x() + m12 * a.y() + m13 * a.z(),
m21 * a.x() + m22 * a.y() + m23 * a.z(),
m31 * a.x() + m32 * a.y() + m33 * a.z()));
}
public void crossedTest()
{
Transform randRot = RandomRotation();
double a = random.NextDouble();
double b = random.NextDouble();
double c = random.NextDouble();
IVector3d va = randRot.transform(Vector3d.xyz(a, 0, 0));
IVector3d vb = randRot.transform(Vector3d.xyz(0, b, 0));
IVector3d vc = randRot.transform(Vector3d.xyz(0, 0, c));
IVector3d w = va.crossed(vb);
double delta = Math.Abs(a * b - w.magnitude());
Assert.AreEqual(a * b, w.magnitude(), EPSILON, $"result vector magnitude not equal to magnitude-product of orthogonal precursor vectors. Delta: {delta}");
Assert.AreEqual(w.dot(va), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(va)}");
Assert.AreEqual(w.dot(vb), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(vb)}");
w = vb.crossed(vc);
delta = Math.Abs(b * c - w.magnitude());
Assert.AreEqual(b * c, w.magnitude(), EPSILON, $"result vector magnitude not equal to magnitude-product of orthogonal precursor vectors. Delta: {delta}");
Assert.AreEqual(w.dot(vb), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(vb)}");
Assert.AreEqual(w.dot(vc), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(vc)}");
w = vc.crossed(va);
delta = Math.Abs(c * a - w.magnitude());
Assert.AreEqual(c * a, w.magnitude(), EPSILON, $"result vector magnitude not equal to magnitude-product of orthogonal precursor vectors. Delta: {delta}");
Assert.AreEqual(w.dot(vc), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(vc)}");
Assert.AreEqual(w.dot(va), 0, EPSILON, $"result vector not perpendicular to precursor vectors. Delta: {w.dot(va)}");
}
public static bool equals(IVector3d thisV, object obj)
{
if (obj == null)
{
return(false);
}
if (thisV.GetType() != obj.GetType())
{
return(false);
}
IVector3d other = (IVector3d)obj;
if (Math.Abs(thisV.x() - other.x()) > Plane.TOL)
{
return(false);
}
if (Math.Abs(thisV.y() - other.y()) > Plane.TOL)
{
return(false);
}
if (Math.Abs(thisV.z() - other.z()) > Plane.TOL)
{
return(false);
}
return(true);
}
///
/// Creates a polygon from the specified point list.
///
/// <param name="points">the points that define the polygon</param>
/// <param name="shared">* @param plane may be null</param>
/// <returns>a polygon defined by the specified point list</returns>
///
private static Polygon fromPoints(
List <IVector3d> points, PropertyStorage shared, Plane plane)
{
IVector3d normal
= (plane != null) ? plane.normal.clone() : null;
if (normal == null)
{
normal = Plane.createFromPoints(
points[0],
points[1],
points[2]).normal;
}
List <Vertex> vertices = new List <Vertex>();
foreach (IVector3d p in points)
{
IVector3d vec = p.clone();
Vertex vertex = new Vertex(vec, normal);
vertices.Add(vertex);
}
return(new Polygon(vertices, shared));
}
public void collinearTest()
{
{
IVector3d p1 = Vector3d.xyz(1, 1, 1);
IVector3d p2 = p1.times(5);
IVector3d p3 = p1.times(10);
Assert.IsTrue(p1.collinear(p2, p3), "p1, p2, p3 must be collinear");
}
{
IVector3d p1 = Vector3d.xyz(1, 1, 1);
IVector3d p2 = p1.times(5, 5, 4);
IVector3d p3 = p1.times(10);
Assert.IsTrue(!p1.collinear(p2, p3), "p1, p2, p3 must not be collinear");
}
{
IVector3d p1 = Vector3d.xyz(10, 10, 10);
IVector3d p2 = Vector3d.xyz(-1, -1, -1);
IVector3d p3 = p1.times(5);
Assert.IsTrue(p1.collinear(p2, p3), "p1, p2, p3 must be collinear");
}
{
IVector3d p1 = Vector3d.xyz(10, 20, 10);
IVector3d p2 = p1.clone();
IVector3d p3 = p2.clone();
Assert.IsTrue(p1.collinear(p2, p3), "p1, p2, p3 must be collinear");
}
}
///
/// Applies the specified transformation to this polygon.
///
/// <b>Note:</b> if the applied transformation performs a mirror operation
/// the vertex order of this polygon is reversed.
///
/// <param name="transform">the transformation to apply</param>
///
/// <returns>this polygon</returns>
///
public Polygon transform(Transform transform)
{
this.vertices.ForEach(
(v) => {
v.transform(transform);
}
);
IVector3d a = this.vertices[0].pos;
IVector3d b = this.vertices[1].pos;
IVector3d c = this.vertices[2].pos;
this._csg_plane.normal = b.minus(a).crossed(c.minus(a)).normalized();
this._csg_plane.dist = this._csg_plane.normal.dot(a);
this.plane = CSharpVecMath.Plane.
fromPointAndNormal(centroid(), _csg_plane.normal);
vertices.ForEach((vertex) => {
vertex.normal = plane.getNormal();
});
if (transform.isMirror())
{
// the transformation includes mirroring. flip polygon
flip();
}
return(this);
}
/// <summary>
/// Determines whether the specified point is in front of, in back of or on
/// this plane.
/// </summary>
///
/// <param name="p">point to check</param>
/// <returns><c>1</c>, if p is in front of the plane, <c>-1</c>, if the
/// point is in the back of this plane and <c>0</c> if the point is on this
/// plane.</returns>
///
public int compare(IVector3d p)
{
// angle between vector n and vector (p-anchor)
double t = this.normal.dot(p.minus(anchor));
return((t < -TOL) ? -1 : (t > TOL) ? 1 : 0);
}
private static void createIntersectionTest(
IVector3d e1p1, IVector3d e1p2,
IVector3d e2p1, IVector3d e2p2,
IVector3d expectedPoint)
{
Edge e1 = new Edge(
new Vertex(
e1p1, Vector3d.Z_ONE),
new Vertex(
e1p2, Vector3d.Z_ONE));
Edge e2 = new Edge(
new Vertex(
e2p1, Vector3d.Z_ONE),
new Vertex(
e2p2, Vector3d.Z_ONE));
IVector3d closestPointResult = e1.getIntersection(e2);
if (expectedPoint != null)
{
Assert.IsTrue(closestPointResult != null, "Intersection point must exist");
IVector3d closestPoint = closestPointResult;
Assert.IsTrue(expectedPoint.Equals(closestPoint), "Intersection point " + expectedPoint + ", got "
+ closestPoint);
}
else
{
Assert.IsFalse(closestPointResult != null, "Intersection point must not exist : "
+ closestPointResult);
}
}
/// <summary>
/// Indicates whether the specified point is contained within this bounding
/// box (check includes box boundary).
/// </summary>
/// <param name="v">vertex to check</param>
/// <returns><c>true</c> if the point is contained within this bounding box;
/// <c>false</c> otherwise
/// </returns>
public bool contains(IVector3d v)
{
bool inX = min.x() <= v.x() && v.x() <= max.x();
bool inY = min.y() <= v.y() && v.y() <= max.y();
bool inZ = min.z() <= v.z() && v.z() <= max.z();
return(inX && inY && inZ);
}
/// <summary>
/// Constructor. Creates a cylinder ranging from <c>start</c> to <c>end</c>
/// with the specified <c>radius</c>. The resolution of the tessellation can
/// be controlled with <c>numSlices</c>.
/// </summary>
///
/// <param name="start">cylinder start</param>
/// <param name="end">cylinder end</param>
/// <param name="radius">cylinder radius</param>
/// <param name="numSlices">number of slices (used for tessellation)</param>
///
public Cylinder(IVector3d start, IVector3d end, double radius, int numSlices)
{
this.start = start;
this.end = end;
this.startRadius = radius;
this.endRadius = radius;
this.numSlices = numSlices;
}
/// <summary>
/// Constructor. Creates a new cylinder with center <c>[0,0,0]</c> and
/// ranging from <c>[0,-0.5,0]</c> to <c>[0,0.5,0]</c>, i.e.
/// <c>size = 1</c>.
/// </summary>
public Cylinder()
{
this.start = Vector3d.xyz(0, -0.5, 0);
this.end = Vector3d.xyz(0, 0.5, 0);
this.startRadius = 1;
this.endRadius = 1;
this.numSlices = 16;
}
/// <summary>
/// Constructor. Creates a cylinder ranging from <c>[0,0,0]</c> to
/// <c>[0,0,height]</c> with the specified <c>radius</c> and
/// <c>height</c>. The resolution of the tessellation can be controlled with
/// <c>numSlices</c>.
/// </summary>
///
/// <param name="startRadius">cylinder start radius</param>
/// <param name="endRadius">cylinder end radius</param>
/// <param name="height">cylinder height</param>
/// <param name="numSlices">number of slices (used for tessellation)</param>
///
public Cylinder(double startRadius, double endRadius, double height, int numSlices)
{
this.start = Vector3d.ZERO;
this.end = Vector3d.Z_ONE.times(height);
this.startRadius = startRadius;
this.endRadius = endRadius;
this.numSlices = numSlices;
}
/// <summary>
/// Adds the specified vector to this vector.
/// </summary>
/// <remarks>
/// This vector is <b>not modified</b>.
/// </remarks>
///
/// <param name="v">the vector to add</param>
/// <returns>this vector</returns>
///
public static IModifiableVector3d add(this IModifiableVector3d vector, IVector3d v)
{
vector.setX(vector.x() + v.x());
vector.setY(vector.y() + v.y());
vector.setZ(vector.z() + v.z());
return(vector);
}
/// <summary>
/// Returns the cross product of this vector and the specified vector.
/// </summary>
/// <remarks>
/// This vector is <b>not modified.</b>
/// </remarks>
///
/// <param name="a">the vector</param>
/// <returns>the cross product of this vector and the specified vector.</returns>
///
public static IVector3d crossed(this IVector3d vector, IVector3d a)
{
return(new Vector3dImpl(
vector.y() * a.z() - vector.z() * a.y(),
vector.z() * a.x() - vector.x() * a.z(),
vector.x() * a.y() - vector.y() * a.x()
));
}
/// <summary>
/// Stores the cross product of this vector and the specified vector in this
/// vector.
/// </summary>
/// <remarks>
/// This vector is <b>modified</b>.
/// </remarks>
///
/// <param name="a">the vector</param>
/// <returns>this vector</returns>
///
public static IModifiableVector3d cross(this IModifiableVector3d vector, IVector3d a)
{
vector.setX(vector.y() * a.z() - vector.z() * a.y());
vector.setY(vector.z() * a.x() - vector.x() * a.z());
vector.setZ(vector.x() * a.y() - vector.y() * a.x());
return(vector);
}
public static int getHashCode(IVector3d v)
{
int hash = 7;
hash = 67 * hash + (int)(BitConverter.DoubleToInt64Bits(v.x()) ^ (BitConverter.DoubleToInt64Bits(v.x()) >> 32));
hash = 67 * hash + (int)(BitConverter.DoubleToInt64Bits(v.y()) ^ (BitConverter.DoubleToInt64Bits(v.y()) >> 32));
hash = 67 * hash + (int)(BitConverter.DoubleToInt64Bits(v.z()) ^ (BitConverter.DoubleToInt64Bits(v.z()) >> 32));
return(hash);
}
/// <summary>
/// Returns the bounds of this csg.
/// </summary>
///
/// <returns>bouds of this csg</returns>
///
public Bounds getBounds()
{
if (polygons.Count == 0)
{
return(new Bounds(Vector3d.ZERO, Vector3d.ZERO));
}
IVector3d initial = polygons[0].vertices[0].pos;
double minX = initial.x();
double minY = initial.y();
double minZ = initial.z();
double maxX = initial.x();
double maxY = initial.y();
double maxZ = initial.z();
foreach (Polygon p in getPolygons())
{
for (int i = 0; i < p.vertices.Count; i++)
{
Vertex vert = p.vertices[i];
if (vert.pos.x() < minX)
{
minX = vert.pos.x();
}
if (vert.pos.y() < minY)
{
minY = vert.pos.y();
}
if (vert.pos.z() < minZ)
{
minZ = vert.pos.z();
}
if (vert.pos.x() > maxX)
{
maxX = vert.pos.x();
}
if (vert.pos.y() > maxY)
{
maxY = vert.pos.y();
}
if (vert.pos.z() > maxZ)
{
maxZ = vert.pos.z();
}
} // end for vertices
} // end for polygon
return(new Bounds(
Vector3d.xyz(minX, minY, minZ),
Vector3d.xyz(maxX, maxY, maxZ)));
}
public IVector3d centroid()
{
IVector3d sum = Vector3d.zero();
foreach (Vertex v in vertices)
{
sum = sum.plus(v.pos);
}
return(sum.times(1.0 / vertices.Count));
}
public void lerpTest()
{
double t = random.NextDouble();
IVector3d va = RandomVector();
IVector3d vb = RandomVector();
IVector3d x = va.lerp(vb, t);
IVector3d tx = va.times(1.0 - t).plus(vb.times(t));
Assert.AreEqual(0, x.distance(tx), EPSILON, $"lerp vector diverges from alternatively generated vector! Delta: {x.distance(tx)}");
}
private Vertex cylPoint(
IVector3d axisX, IVector3d axisY, IVector3d axisZ, IVector3d ray, IVector3d s,
double r, double stack, double slice, double normalBlend)
{
double angle = slice * Math.PI * 2;
IVector3d ot = axisX.times(Math.Cos(angle)).plus(axisY.times(Math.Sin(angle)));
IVector3d pos = s.plus(ray.times(stack)).plus(ot.times(r));
IVector3d normal = ot.times(1.0 - Math.Abs(normalBlend)).plus(axisZ.times(normalBlend));
return(new Vertex(pos, normal));
}
public void orthogonalTest()
{
Transform randRot = RandomRotation();
double a = 100.0 * random.NextDouble();
IVector3d va = randRot.transform(Vector3d.X_ONE.times(a));
IVector3d oa = va.orthogonal();
double delta = Math.Abs(oa.dot(va));
Assert.AreEqual(0, oa.dot(va), EPSILON, $"dotproduct of orthogonal vector diverges from zero! Delta:{delta}");
}
private Vertex sphereVertex(IVector3d c, double r, double theta, double phi)
{
theta *= Math.PI * 2;
phi *= Math.PI;
IVector3d dir = Vector3d.xyz(
Math.Cos(theta) * Math.Sin(phi),
Math.Cos(phi),
Math.Sin(theta) * Math.Sin(phi)
);
return(new Vertex(c.plus(dir.times(r)), dir));
}
/// <summary>
/// Applies this transform to the specified vector.
/// </summary>
///
/// <param name="vec">vector to transform</param>
/// <returns>the specified vector</returns>
///
public IVector3d transform(IVector3d vec)
{
IModifiableVector3d result = vec.asModifiable();
double x, y;
x = m.m00 * vec.x() + m.m01 * vec.y() + m.m02 * vec.z() + m.m03;
y = m.m10 * vec.x() + m.m11 * vec.y() + m.m12 * vec.z() + m.m13;
result.setZ(m.m20 * vec.x() + m.m21 * vec.y() + m.m22 * vec.z() + m.m23);
result.setX(x);
result.setY(y);
return(result);
}
public void negatedTest()
{
IVector3d v = RandomVector().times(100.0);
IVector3d n = v.negated();
double delta = Math.Abs(v.magnitude() - n.magnitude());
Assert.AreEqual(v.magnitude(), n.magnitude(), EPSILON, $"calculated magnitudes diverges for negated vector! Delta: {delta}");
IVector3d z = v.plus(n);
Assert.AreEqual(z.magnitude(), 0, EPSILON, $"sum of vector and its negation not equal to zero! Delta: {z.magnitude()}");
}
/// <summary>
/// Applies a rotation operation about the specified rotation axis.
/// </summary>
///
/// <param name="axisPos">axis point</param>
/// <param name="axisDir">axis direction (may be unnormalized)</param>
/// <param name="degrees">rotantion angle in degrees</param>
/// <returns>this transform</returns>
///
public Transform rot(IVector3d axisPos, IVector3d axisDir, double degrees)
{
Transform tmp = Transform.unity();
axisDir = axisDir.normalized();
IVector3d dir2 = axisDir.times(axisDir);
double posx = axisPos.x();
double posy = axisPos.y();
double posz = axisPos.z();
double dirx = axisDir.x();
double diry = axisDir.y();
double dirz = axisDir.z();
double dirxSquare = dir2.x();
double dirySquare = dir2.y();
double dirzSquare = dir2.z();
double radians = degrees * Math.PI * (1.0 / 180.0);
double cosOfAngle = Math.Cos(radians);
double oneMinusCosOfangle = 1 - cosOfAngle;
double sinOfangle = Math.Sin(radians);
tmp.m.m00 = dirxSquare + (dirySquare + dirzSquare) * cosOfAngle;
tmp.m.m01 = dirx * diry * oneMinusCosOfangle - dirz * sinOfangle;
tmp.m.m02 = dirx * dirz * oneMinusCosOfangle + diry * sinOfangle;
tmp.m.m03 = (posx * (dirySquare + dirzSquare)
- dirx * (posy * diry + posz * dirz)) * oneMinusCosOfangle
+ (posy * dirz - posz * diry) * sinOfangle;
tmp.m.m10 = dirx * diry * oneMinusCosOfangle + dirz * sinOfangle;
tmp.m.m11 = dirySquare + (dirxSquare + dirzSquare) * cosOfAngle;
tmp.m.m12 = diry * dirz * oneMinusCosOfangle - dirx * sinOfangle;
tmp.m.m13 = (posy * (dirxSquare + dirzSquare)
- diry * (posx * dirx + posz * dirz)) * oneMinusCosOfangle
+ (posz * dirx - posx * dirz) * sinOfangle;
tmp.m.m20 = dirx * dirz * oneMinusCosOfangle - diry * sinOfangle;
tmp.m.m21 = diry * dirz * oneMinusCosOfangle + dirx * sinOfangle;
tmp.m.m22 = dirzSquare + (dirxSquare + dirySquare) * cosOfAngle;
tmp.m.m23 = (posz * (dirxSquare + dirySquare)
- dirz * (posx * dirx + posy * diry)) * oneMinusCosOfangle
+ (posx * diry - posy * dirx) * sinOfangle;
apply(tmp);
return(this);
}
public void distanceTest()
{
Transform randTrans = RandomRotation();
double distance = 100.0 * random.NextDouble();
IVector3d d = randTrans.transform(Vector3d.X_ONE.times(distance));
IVector3d a = RandomVector().times(100.0);
IVector3d b = a.plus(d);
double calcDistance = a.distance(b);
double delta = Math.Abs(calcDistance - distance);
Assert.AreEqual(distance, calcDistance, EPSILON, $"calculated distance diverges from generated distance! Delta: {delta}");
}
/// <summary>
/// Creates a plane defined by the the specified points. The anchor point of
/// the plane is the centroid of the triangle (a,b,c).
/// </summary>
///
/// <param name="a">first point</param>
/// <param name="b">second point</param>
/// <param name="c">third point</param>
/// <returns>a plane</returns>
///
public static Plane fromPoints(IVector3d a, IVector3d b, IVector3d c)
{
IVector3d normal = b.minus(a).crossed(c.minus(a)).normalized();
IVector3d anchor = Vector3d.zero();
anchor = anchor.plus(a);
anchor = anchor.plus(b);
anchor = anchor.plus(c);
anchor = anchor.times(1.0 / 3.0);
return(new Plane(anchor, normal));
}
public IVector3d Add (IVector3d _rhs) {
var rhs = (Vector3d)_rhs;
return new Vector3d(x + rhs.x, y + rhs.y, z + rhs.z);
}
