public static ICurve CreateTrimmedLine(this IModeler modeler, Vector3 p0, Vector3 v0, double length)
{
v0 = v0.Unit() * (double)length;
var p1 = p0 + v0;
return(CreateTrimmedLine(modeler, p0, p1));
}
/// <summary>
/// Project a point onto this plane
/// </summary>
/// <param name="q"></param>
/// <returns></returns>
public Vector3 ProjectOnPlane(Vector3 q)
{
var p = Point;
var dUnit = Direction.Unit();
return(q - (q - p).Dot(dUnit) * dUnit);
}
/// <summary>
/// 创建直线
/// </summary>
/// <param name="modeler">造型接口</param>
/// <param name="math">数学计算接口</param>
/// <param name="p0">起点</param>
/// <param name="v0">方向</param>
/// <param name="length">长度</param>
/// <returns></returns>
public static ICurve CreateTrimmedLine(this IModeler modeler, MathUtility math, Vector3 p0, Vector3 v0, double length)
{
v0 = v0.Unit().Scale(length);
var p1 = p0.Add(v0);
return(CreateTrimmedLine(modeler, p0.ToSwMathPoint(math), p1.ToSwMathPoint(math)));
}
protected override Vector3 Closest(Vector3 p)
{
if (IsCollided(p))
{
return(p);
}
else
{
return(radius * Vector3.Unit(p - this.position) + this.position);
}
}
public static IBody2 CreateSemiCirclularSheet
(this IModeler modeler
, Vector3 center
, Vector3 vNormal
, Vector3 vRef // Horizontal
, double radius)
{
// Should be orthogonal
Debug.Assert(vRef.Dot(vNormal) < 1e-9);
var math = SwAddinBase.Active.Math;
var centerSw = center.ToSwMathPoint();
var vNormalSw = vNormal.ToSwMathPoint();
var vNormalOrthSw = vRef.ToSWVector(math).Normalise();
var centerDbls = centerSw.ArrayData;
var vNormalDbls = vNormalSw.ArrayData;
var vNormalOrthDbls = vNormalOrthSw.ArrayData;
var surf = (Surface)modeler.CreatePlanarSurface2(centerDbls, vNormalDbls, vNormalOrthDbls);
var startPoint = center + radius * vRef.Unit();
var endPoint = center - radius * vRef.Unit();
var startPointDbls = startPoint.ToDoubles();
var endPointDbls = endPoint.ToDoubles();
var arco = modeler.CreateArc
(centerDbls, vNormalDbls, radius, startPointDbls, endPointDbls);
var arc = (Curve)arco;
var arcStartPoint = startPoint;
var arcEndPoint = endPoint;
var trimmedArc = arc.CreateTrimmedCurve2(arcStartPoint.X, arcStartPoint.Y, arcStartPoint.Z, arcEndPoint.X, arcEndPoint.Y, arcEndPoint.Z);
var line = modeler.CreateTrimmedLine(arcEndPoint, arcStartPoint);
return((IBody2)surf.CreateTrimmedSheet(new[] { trimmedArc, line }));
}
public static Vector3 rnd_dome(Vector3 normal)
{
var p = new Vector3();
float d;
do
{
p.x = (float)(2.0 * rnd.NextDouble() - 1.0);
p.y = (float)(2.0 * rnd.NextDouble() - 1.0);
p.z = (float)(2.0 * rnd.NextDouble() - 1.0);
p = p.Unit();
d = p.Dot(normal);
} while (d < 0);
return(p);
}
public static Vector3 rnd_dome(Vector3 normal, Random rnd)
{
var p = new Vector3();
float d;
do
{
p.x = 2.0f * rnd.NextFloat() - 1.0f;
p.y = 2.0f * rnd.NextFloat() - 1.0f;
p.z = 2.0f * rnd.NextFloat() - 1.0f;
p = p.Unit();
d = p.Dot(normal);
} while (d < 0);
return(p);
}
/// <summary>
/// Calculates the face normals for the current subSet in object space.
/// </summary>
/// <returns>The face normals for the current subSet.</returns>
public Vector3[] CalcFaceNormalsOld()
{
Vector3[] faceNormals = new Vector3[this.PrimitiveCount];
PositionStream positionStream = (PositionStream)vertexUnit[typeof(PositionStream)];
for (int i = 0; i < this.PrimitiveCount; i++)
{
int index0 = indexStream[i * 3];
int index1 = indexStream[i * 3 + 1];
int index2 = indexStream[i * 3 + 2];
Vector3 v0 = positionStream[index0];
Vector3 v1 = positionStream[index1];
Vector3 v2 = positionStream[index2];
faceNormals[i] = Vector3.Unit(Vector3.Cross(v2 - v0, v1 - v0));
}
return(faceNormals);
}
public static Vector3 RandomDome(Vector3 normal)
{
Vector3 p;
float d;
do
{
p = new Vector3(
(2.0f * Random.NextFloat() - 1.0f),
(2.0f * Random.NextFloat() - 1.0f),
(2.0f * Random.NextFloat() - 1.0f)
);
p = p.Unit();
d = Vector3.Dot(p, normal);
} while (d < 0);
return(p);
}
/// <summary>
/// 单位化
/// </summary>
/// <returns></returns>
public Vector3 Unit()
{
return(_Vector.Unit());
}
public float GetIntensity(Vector3 surfaceNormal, Vector3 p)
{
return(Vector3.Dot(surfaceNormal.Unit(), (location - p).Unit()) * intensity / (location - p).Magnitude());
}
public static void Collide(SimObject obj1, SimObject obj2)
{
Vector3 collisionPoint = (obj1.Closest(obj2.position) + obj2.Closest(obj1.position)) / 2;
Vector3 normal = (obj1.Normal(collisionPoint) - obj2.Normal(collisionPoint)) / 2;
double e = obj1.restitution * obj2.restitution;
//transform to relative to obj2 for simplicity
Vector3 u = obj1.velocity - obj2.velocity;
double ucostheta = Vector3.dot(normal, u);
// Done on paper, quadratic gives 2 solutions but only 1 is correct
Vector3 v2_1 = (ucostheta * (1 + e) / (1 + obj2.mass / obj1.mass)) * Vector3.Unit(u);
Vector3 v2_2 = (ucostheta * (1 - e) / (1 + obj2.mass / obj1.mass)) * Vector3.Unit(u);
Vector3 v1_1 = (ucostheta - Vector3.Magnitude(v2_1) * obj2.mass / obj1.mass) * Vector3.Unit(u);
Vector3 v1_2 = (ucostheta - Vector3.Magnitude(v2_2) * obj2.mass / obj1.mass) * Vector3.Unit(u);
double check_1 = Vector3.dot(normal, v1_1 - v2_1);
double check_2 = Vector3.dot(normal, v1_2 - v2_2);
Vector3 v1;
Vector3 v2;
if (ucostheta < 0)
{
if (check_1 > 0)
{
if (check_2 > 0)
{
throw new Exception("Mechanics incorrect");
}
else
{
v1 = v1_1;
v2 = v2_1;
}
}
else if (check_2 > 0)
{
v1 = v1_2;
v2 = v2_2;
}
else
{
throw new Exception("Mechanics incorrect");
}
}
else
{
if (check_1 < 0)
{
if (check_2 < 0)
{
throw new Exception("Mechanics incorrect");
}
else
{
v1 = v1_1;
v2 = v2_1;
}
}
else if (check_2 < 0)
{
v1 = v1_2;
v2 = v2_2;
}
else
{
throw new Exception("Mechanics incorrect");
}
}
// transform back to the real world
obj1.velocity = obj2.velocity + v1;
obj2.velocity = obj2.velocity + v2;
}
public static Vector3 rnd_dome (Vector3 normal) {
var p = new Vector3();
float d;
do {
p.x = (float)(2.0 * rnd.NextDouble() - 1.0);
p.y = (float)(2.0 * rnd.NextDouble() - 1.0);
p.z = (float)(2.0 * rnd.NextDouble() - 1.0);
p = p.Unit();
d = p.Dot(normal);
} while (d < 0);
return p;
}
protected override Vector3 Normal(Vector3 p)
{
// normal vector is just the vector from the center to the surface
return(Vector3.Unit(p - this.position));
}
public static Vector3 ProjectOn(this Vector3 point, Vector3 axis)
{
return(ProjectOnUnit(point, axis.Unit()));
}
public static Vector3 ProjectOn(this Vector3 point, Vector3 axis)
{
return(axis.Unit() * Vector3.Dot(point, axis));
}
/// <summary>
/// calculate tangent, normal and binormal stream
/// Triangle list is assumed for indices format
/// there will be a more sophisticated version of this functions for SubSets, which will
/// have much similiarity to NVidias MeshMender
/// </summary>
/// <param name="positions">stream containing vertex positions</param>
/// <param name="textures">stream containing texture coordinates</param>
/// <param name="indices">stream containing indices</param>
/// <returns></returns>
public static IGraphicsStream[] CalcTangentSpaceStreams(VertexStreams.PositionStream positions,
VertexStreams.TextureStream textures,
VertexStreams.IndexStream indices)
{
// create ret streams
VertexStreams.NormalStream normal = new VertexStreams.NormalStream(positions.Size);
VertexStreams.TangentStream tangent = new VertexStreams.TangentStream(positions.Size);
VertexStreams.BinormalStream binormal = new VertexStreams.BinormalStream(positions.Size);
for (int i = 0; i < indices.Size / 3; i++)
{
// prepare data
Vector3 s = Vector3.Zero;
Vector3 t = Vector3.Zero;
Triangle tri = indices.GetTriangle(i);
// calculate s.X and t.X
Vector3 a = new Vector3(positions[tri.B].X - positions[tri.A].X,
textures[tri.B].X - textures[tri.A].X,
textures[tri.B].Y - textures[tri.A].Y);
Vector3 b = new Vector3(positions[tri.C].X - positions[tri.A].X,
textures[tri.C].X - textures[tri.A].X,
textures[tri.C].Y - textures[tri.A].Y);
Vector3 axb = Vector3.Cross(a, b);
if (Basic.Abs(axb.X) > float.Epsilon)
{
s.X = -axb.Y / axb.X;
t.X = -axb.Z / axb.X;
}
// calculate s.Y and t.Y
a.X = positions[tri.B].Y - positions[tri.A].Y;
b.X = positions[tri.C].Y - positions[tri.A].Y;
axb = Vector3.Cross(a, b);
if (Basic.Abs(axb.X) > float.Epsilon)
{
s.Y = -axb.Y / axb.X;
t.Y = -axb.Z / axb.X;
}
// calculate s.Z and t.Z
a.X = positions[tri.B].Z - positions[tri.A].Z;
b.X = positions[tri.C].Z - positions[tri.A].Z;
axb = Vector3.Cross(a, b);
if (Basic.Abs(axb.X) > float.Epsilon)
{
s.Z = -axb.Y / axb.X;
t.Z = -axb.Z / axb.X;
}
// normalize and calculate normal vector
s.Normalize();
t.Normalize();
// swap t if normal vector of texture space triangle has a negative z direction
if (axb.X < float.Epsilon)
{
t = -t;
}
Vector3 sxt = Vector3.Unit(Vector3.Cross(s, t));
// add s, t, sxt into the streams
// to get the average value - in a later version I will add functionality
// to duplicate vertices if the results for one vertex are too different
// (like the NVidia MeshMender does ...)
tangent[tri.A] += s;
tangent[tri.B] += s;
tangent[tri.C] += s;
binormal[tri.A] += t;
binormal[tri.B] += t;
binormal[tri.C] += t;
normal[tri.A] += sxt;
normal[tri.B] += sxt;
normal[tri.C] += sxt;
}
// so last but not least renormalize the summed vectors
for (int i = 0; i < positions.Size; i++)
{
tangent[i].Normalize();
binormal[i].Normalize();
normal[i].Normalize();
}
return(new IGraphicsStream[] { tangent, binormal, normal });
}
