| public static Scale ( Vector3D, a, Vector3D, b ) : Vector3D, | ||
| a | Vector3D, | |
| b | Vector3D, | |
| return | Vector3D, | |
// Add a normal for a polygon.
// Assumes the points are coplanar.
// Assumes the points are outwardly oriented.
public static void AddPolygonNormal(this MeshGeometry3D mesh,
double length, double thickness, params Point3D[] points)
{
// Find the "center" point.
double x = 0, y = 0, z = 0;
foreach (Point3D point in points)
{
x += point.X;
y += point.Y;
z += point.Z;
}
Point3D endpoint1 = new Point3D(
x / points.Length,
y / points.Length,
z / points.Length);
// Get the polygon's normal.
Vector3D n = FindTriangleNormal(
points[0], points[1], points[2]);
// Set the length.
n = n.Scale(length);
// Find the segment's other end point.
Point3D endpoint2 = endpoint1 + n;
// Create the segment.
AddSegment(mesh, endpoint1, endpoint2, thickness);
}
public static void AddSegment(this MeshGeometry3D mesh,
Point3D point1, Point3D point2, Vector3D up, double thickness,
bool extend)
{
// Get the segment's vector.
Vector3D v = point2 - point1;
if (extend)
{
// Increase the segment's length on both ends by thickness / 2.
Vector3D n = v.Scale(thickness / 2.0);
point1 -= n;
point2 += n;
v += 2 * n;
}
// Get the scaled up vector.
Vector3D n1 = up.Scale(thickness / 2.0);
// Get another scaled perpendicular vector.
Vector3D n2 = Vector3D.CrossProduct(v, n1);
n2 = n2.Scale(thickness / 2.0);
// Make a skinny box.
mesh.AddBox(point1 - n1 - n2, v, 2 * n1, 2 * n2);
}
static HDCallbackCode GravityWellCallback(IntPtr pUserData)
{
const double kStiffness = 0.175; //(N/mm)
const double kGravityWellInfluence = 50; //Box Size:50x50x50(mm)
Vector3D position = new Vector3D();
Vector3D force = new Vector3D();
Vector3D positionTwell = new Vector3D();
uint hHD = HDAPI.hdGetCurrentDevice();
//触觉技术框架开始。(一般来说，所有与状态相关的触觉调用都应该在一个框架内进行。)
HDAPI.hdBeginFrame(hHD);
//获取设备的当前位置
HDAPI.hdGetDoublev(HDGetParameters.HD_CURRENT_POSITION, ref position);
//Console.WriteLine("Vector3D:{0} {1} {2}", position.X, position.Y, position.Z);
force.ResetZero();
// positionTwell = wellPos - position
//创建一个从设备位置到重力井中心的矢量
Vector3D.Subtrace(ref positionTwell, ref wellPos, ref position);
//如果装置位置在重力井中心一定距离内，则向重力井中心施加弹簧力。
//力的计算不同于传统的重力体，因为装置离中心越近，井所施加的力就越小;
//该装置的行为就像弹簧连接在它自己和井的中心。
if (InForce && Vector3D.Magnitude(ref positionTwell) < kGravityWellInfluence)
{
// F = k * x
//F:力的单位为牛顿(N)
//k: 井的刚度(N / mm)
//x: 从设备端点位置到井中心的向量。
Vector3D.Scale(ref force, ref positionTwell, kStiffness);
}
if (!InForce && Vector3D.Magnitude(ref positionTwell) >= kGravityWellInfluence)
{
Vector3D.Scale(ref force, ref positionTwell, kStiffness);
}
//把力传送到设备上
HDAPI.hdSetDoublev(HDSetParameters.HD_CURRENT_FORCE, ref force);
//End haptics frame.
HDAPI.hdEndFrame(hHD);
//check error
HDErrorInfo error = HDAPI.hdGetError();
if (error.CheckedError())
{
Console.WriteLine("渲染重力时检测到错误");
if (error.IsSchedulerError())
{
Console.WriteLine("调度器错误。");
return(HDCallbackCode.HD_CALLBACK_DONE);
}
}
return(HDCallbackCode.HD_CALLBACK_CONTINUE);
}
public void can_get_scaled_vector()
{
var actual = new Vector3D(1, -2, 4);
var expected = new Vector3D(3, -6, 12);
actual.Scale(3);
Assert.Equal(expected, actual);
}
public static void AddSegment(MeshGeometry3D mesh,
Point3D point1, Point3D point2, Vector3D up, double thickness,
bool extend)
{
// Get the segment's vector.
Vector3D v = point2 - point1;
if (extend)
{
// Increase the segment's length on both ends by thickness / 2.
Vector3D n = v.Scale(thickness / 2.0);
point1 -= n;
point2 += n;
}
// Get the scaled up vector.
Vector3D n1 = up.Scale(thickness / 2.0);
// Get another scaled perpendicular vector.
Vector3D n2 = Vector3D.CrossProduct(v, n1);
n2 = n2.Scale(thickness / 2.0);
// Make a skinny box.
// p1pm means point1 PLUS n1 MINUS n2.
Point3D p1pp = point1 + n1 + n2;
Point3D p1mp = point1 - n1 + n2;
Point3D p1pm = point1 + n1 - n2;
Point3D p1mm = point1 - n1 - n2;
Point3D p2pp = point2 + n1 + n2;
Point3D p2mp = point2 - n1 + n2;
Point3D p2pm = point2 + n1 - n2;
Point3D p2mm = point2 - n1 - n2;
// Sides.
AddTriangle(mesh, p1pp, p1mp, p2mp);
AddTriangle(mesh, p1pp, p2mp, p2pp);
AddTriangle(mesh, p1pp, p2pp, p2pm);
AddTriangle(mesh, p1pp, p2pm, p1pm);
AddTriangle(mesh, p1pm, p2pm, p2mm);
AddTriangle(mesh, p1pm, p2mm, p1mm);
AddTriangle(mesh, p1mm, p2mm, p2mp);
AddTriangle(mesh, p1mm, p2mp, p1mp);
// Ends.
AddTriangle(mesh, p1pp, p1pm, p1mm);
AddTriangle(mesh, p1pp, p1mm, p1mp);
AddTriangle(mesh, p2pp, p2mp, p2mm);
AddTriangle(mesh, p2pp, p2mm, p2pm);
}
public void Scale_MultipliedByVector_ExpectCorrectNewPoint()
{
//arrange
var scalars = new Vector3D(2.0d, 3.0d, 4.0d);
var point = new Vector3D(-4.0d, 6.0d, 8.0d);
//act
var newPoint = point.Scale(scalars);
//assert
Assert.AreEqual(new Vector3D(-8.0d, 18.0d, 32.0d), newPoint);
}
// Add a segment representing the triangle's normal to the normals mesh.
private static void AddTriangleNormal(MeshGeometry3D mesh,
MeshGeometry3D normals, Point3D point1, Point3D point2, Point3D point3,
double length, double thickness)
{
// Get the triangle's normal.
Vector3D n = FindTriangleNormal(point1, point2, point3);
// Set the length.
n = n.Scale(length);
// Find the center of the triangle.
Point3D endpoint1 = new Point3D(
(point1.X + point2.X + point3.X) / 3.0,
(point1.Y + point2.Y + point3.Y) / 3.0,
(point1.Z + point2.Z + point3.Z) / 3.0);
// Find the segment's other end point.
Point3D endpoint2 = endpoint1 + n;
// Create the segment.
AddSegment(normals, endpoint1, endpoint2, thickness);
}
// Move the point to the indicated distance away from the center.
private static void NormalizePoint(ref Point3D point, Point3D center, double radius)
{
Vector3D vector = point - center;
point = center + vector.Scale(radius);
}
public override void Update(float deltaTime, uint tick)
{
for (int i = 0; i < _pathToPositionComponentArray.Length; i++)
{
//C'mon. If the dude isn't active you shouldn't do a damn thing. Get out of herer
if (!_entityManager.IsEntityActive(i))
{
return;
}
//Now that we know this is an active Entity, does it even have a path?
if (_pathToPositionComponentArray[i].IsActive)
{
//There's a path. Now, what to do?
//This function should use the entities move speed to move them along the path.
//It should move along the path segment by segment.
//Move the motherf*@!cker
//Grab a short hand name for this array element, other wise the following code would be waaaaay longer
PathToPositionComponent ptpc = _pathToPositionComponentArray[i];
//Get this doods move speed
float moveSpeed = _combatStatsComponents[i].MovementSpeed;
//Grab ye olde vector from this entity to the target position
Vector3D moveDeltaVector = ptpc.Path[ptpc.CurrentPathSegmentIndex] - _positionComponentArray[i].Position;
//Normalize the vector, then scale it according to the move speed
moveDeltaVector.Normalize();
//This is cool. This normalized vector is our direction, so just go ahead and record that
_positionComponentArray[i].Direction = moveDeltaVector;
moveDeltaVector.Scale(moveSpeed * deltaTime);
//Now add the delta vector to our position
_positionComponentArray[i].Position.Add(moveDeltaVector);
_logger.Log(string.Format("Entity {0} moved to {1}, {2}, {3}", i, _positionComponentArray[i].Position.X, _positionComponentArray[i].Position.Y, _positionComponentArray[i].Position.Z));
//We should check to see if we're at the end point of a path segment OR we've moved beyond our target point
//First, get the distance
float dist = _positionComponentArray[i].Position.DistanceTo(ptpc.Path[ptpc.CurrentPathSegmentIndex]);
//Now get the dot product to know if we're beyond our target point
Vector3D postMoveVector = ptpc.Path[ptpc.CurrentPathSegmentIndex] - _positionComponentArray[i].Position;
float dotProd = Vector3D.DotProduct(postMoveVector, _positionComponentArray[i].Direction);
//Now, we can compare this squared distance to our squared epsilon. This saves us from having to perform a square root calculation
if (dist <= _combatStatsComponents[i].AttackRange || dotProd <= 0.0f)
{
//We can now consider ourselves "arrived" at this point
//Set the current position to this point, and then up the current path index. We also need to check to see if this was the last path segment
//and if so, consider our travel along this path as complete.
_positionComponentArray[i].Position.Set(ptpc.Path[ptpc.CurrentPathSegmentIndex]);
if (ptpc.CurrentPathSegmentIndex == ptpc.Path.Length - 1)
{
//Oh boy, we're at the end. What a treat.
//Lets remove the path component, because that's within our power
_pathToPositionComponentArray[i].Path = null;
_pathToPositionComponentArray[i].IsActive = false;
_logger.Log(string.Format("Entity {0} arrived at end of path!", i));
}
else
{
//Not the end mf. Press on.
//Update our index to the next segment... and that's cool.
ptpc.CurrentPathSegmentIndex++;
}
}
}
}
}
// Draw a branch in the indicated direction.
private void DrawBranch(MeshGeometry3D mesh,
int depth, double length, double radius, double angle,
double lengthFactor, double radiusFactor, int numBranches,
Point3D startPoint, Vector3D axis, int numSides)
{
// Set the branch's length.
axis = axis.Scale(length);
// Get two vectors perpendicular to the axis.
Vector3D v1;
if (Vector3D.AngleBetween(axis, D3.ZVector()) > 0.1)
{
v1 = Vector3D.CrossProduct(axis, D3.ZVector());
}
else
{
v1 = Vector3D.CrossProduct(axis, D3.XVector());
}
Vector3D v2 = Vector3D.CrossProduct(axis, v1);
v1 = v1.Scale(radius);
v2 = v2.Scale(radius);
// Make the cone's base.
Point3D[] pgon = G3.MakePolygonPoints(numSides, startPoint, v1, v2);
// Make the branch.
mesh.AddCylinder(pgon, axis, true);
// Draw child branches.
if (depth <= 0)
{
return;
}
// Move to the end of this segment.
depth--;
startPoint += axis;
length *= lengthFactor;
if (length < 0.01)
{
length = 0.01;
}
radius *= radiusFactor;
if (radius < 0.001)
{
radius = 0.001;
}
// Find child vectors.
List <Vector3D> children = axis.MakeFlowerVectors(
D3.YVector(), D3.ZVector(), angle, numBranches);
// Draw the child branches.
foreach (Vector3D child in children)
{
DrawBranch(mesh, depth, length,
radius, angle, lengthFactor, radiusFactor, numBranches,
startPoint, child, numSides);
}
}
