/// <summary>
/// Initializes a new instance of the <see cref="TiledNavMesh"/> class.
/// </summary>
/// <param name="data">The Navigation Mesh data</param>
public TiledNavMesh(NavMeshBuilder data)
{
this.origin = data.Header.Bounds.Min;
this.tileWidth = data.Header.Bounds.Max.X - data.Header.Bounds.Min.X;
this.tileHeight = data.Header.Bounds.Max.Z - data.Header.Bounds.Min.Z;
this.maxTiles = 1;
this.maxPolys = data.Header.PolyCount;
//init tiles
tileSet = new Dictionary<Vector2i, List<MeshTile>>();
tileRefs = new Dictionary<MeshTile, PolyId>();
tileList = new List<MeshTile>();
//init ID generator values
int tileBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(maxTiles));
int polyBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(maxPolys));
//only allow 31 salt bits, since salt mask is calculated using 32-bit int and it will overflow
int saltBits = Math.Min(31, 32 - tileBits - polyBits);
//TODO handle this in a sane way/do we need this?
if (saltBits < 10)
return;
idManager = new PolyIdManager(polyBits, tileBits, saltBits);
AddTile(data);
}
internal static bool AlmostEqual(ref Vector3 a, ref Vector3 b, float threshold)
{
float threshSq = threshold * threshold;
float distSq = (b - a).LengthSquared();
return distSq < threshold;
}
/// <summary>
/// Initializes a new instance of the <see cref="SharpNav.ContourVertex"/> struct.
/// </summary>
/// <param name="vec">The array of X,Y,Z coordinates.</param>
/// <param name="region">The Region ID.</param>
public ContourVertex(Vector3 vec, RegionId region)
{
this.X = (int)vec.X;
this.Y = (int)vec.Y;
this.Z = (int)vec.Z;
this.RegionId = region;
}
public TiledNavMesh(Vector3 origin, float tileWidth, float tileHeight, int maxTiles, int maxPolys)
{
this.origin = origin;
this.tileWidth = tileWidth;
this.tileHeight = tileHeight;
this.maxTiles = maxTiles;
this.maxPolys = maxPolys;
//init tiles
tileSet = new Dictionary<Vector2i, List<MeshTile>>();
tileRefs = new Dictionary<MeshTile, PolyId>();
tileList = new List<MeshTile>();
//init ID generator values
int tileBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(maxTiles));
int polyBits = MathHelper.Log2(MathHelper.NextPowerOfTwo(maxPolys));
//only allow 31 salt bits, since salt mask is calculated using 32-bit int and it will overflow
int saltBits = Math.Min(31, 32 - tileBits - polyBits);
//TODO handle this in a sane way/do we need this?
if (saltBits < 10)
return;
idManager = new PolyIdManager(polyBits, tileBits, saltBits);
}
public DungeonRoom(Vector3 size, Vector3 position)
{
this.size = size;
this.position = position;
entries = new List<Tuple<int, int>>();
loaded = false;
}
public virtual float GetCost(Vector3 a, Vector3 b,
NavPolyId prevRef, NavTile prevTile, NavPoly prevPoly,
NavPolyId curRef, NavTile curTile, NavPoly curPoly,
NavPolyId nextRef, NavTile nextTile, NavPoly nextPoly)
{
return (a - b).Length() * areaCost[(int)curPoly.Area.Id];
}
/// <summary>
/// Determine whether a ray (origin, dir) is intersecting a segment AB.
/// </summary>
/// <param name="origin">The origin of the ray.</param>
/// <param name="dir">The direction of the ray.</param>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="t">The parameter t</param>
/// <returns>A value indicating whether the ray is intersecting with the segment.</returns>
public static bool RaySegment(Vector3 origin, Vector3 dir, Vector3 a, Vector3 b, out float t)
{
//default if not intersectng
t = 0;
Vector3 v = b - a;
Vector3 w = origin - a;
float d;
Vector3Extensions.PerpDotXZ(ref dir, ref v, out d);
d *= -1;
if (Math.Abs(d) < 1e-6f)
return false;
d = 1.0f / d;
Vector3Extensions.PerpDotXZ(ref v, ref w, out t);
t *= -d;
if (t < 0 || t > 1)
return false;
float s;
Vector3Extensions.PerpDotXZ(ref dir, ref w, out s);
s *= -d;
if (s < 0 || s > 1)
return false;
return true;
}
/// <summary>
/// Rasterizes several triangles at once from an indexed array with per-triangle area flags.
/// </summary>
/// <param name="verts">An array of vertices.</param>
/// <param name="inds">An array of indices.</param>
/// <param name="vertOffset">An offset into the vertex array.</param>
/// <param name="vertStride">The number of array elements that make up a vertex. A value of 0 is interpreted as tightly-packed data (one Vector3 per vertex).</param>
/// <param name="indexOffset">An offset into the index array.</param>
/// <param name="triCount">The number of triangles to rasterize.</param>
/// <param name="areas">An array of area flags, one for each triangle.</param>
public void RasterizeTrianglesIndexedWithAreas(Vector3[] verts, int[] inds, int vertOffset, int vertStride, int indexOffset, int triCount, Area[] areas)
{
int indexEnd = triCount * 3 + indexOffset;
if (verts == null)
throw new ArgumentNullException("verts");
if (inds == null)
throw new ArgumentNullException("inds");
if (indexEnd > inds.Length)
throw new ArgumentOutOfRangeException("indexCount", "The specified index offset and length end outside the provided index array.");
if (vertOffset < 0)
throw new ArgumentOutOfRangeException("vertOffset", "vertOffset must be greater than or equal to 0.");
if (vertStride < 0)
throw new ArgumentOutOfRangeException("vertStride", "vertStride must be greater than or equal to 0.");
else if (vertStride == 0)
vertStride = 1;
if (areas.Length < triCount)
throw new ArgumentException("There must be at least as many AreaFlags as there are triangles.", "areas");
for (int i = indexOffset, j = 0; i < indexEnd; i += 3, j++)
{
int indA = inds[i] * vertStride + vertOffset;
int indB = inds[i + 1] * vertStride + vertOffset;
int indC = inds[i + 2] * vertStride + vertOffset;
RasterizeTriangle(ref verts[indA], ref verts[indB], ref verts[indC], areas[j]);
}
}
/// <summary>
/// Find the 3D distance between a point (x, y, z) and a segment PQ
/// </summary>
/// <param name="pt">The coordinate of the point.</param>
/// <param name="p">The coordinate of point P in the segment PQ.</param>
/// <param name="q">The coordinate of point Q in the segment PQ.</param>
/// <returns>The distance between the point and the segment.</returns>
internal static float PointToSegmentSquared(ref Vector3 pt, ref Vector3 p, ref Vector3 q)
{
//distance from P to Q
Vector3 pq = q - p;
//disance from P to the lone point
float dx = pt.X - p.X;
float dy = pt.Y - p.Y;
float dz = pt.Z - p.Z;
float segmentMagnitudeSquared = pq.LengthSquared();
float t = pq.X * dx + pq.Y * dy + pq.Z * dz;
if (segmentMagnitudeSquared > 0)
t /= segmentMagnitudeSquared;
//keep t between 0 and 1
if (t < 0)
t = 0;
else if (t > 1)
t = 1;
dx = p.X + t * pq.X - pt.X;
dy = p.Y + t * pq.Y - pt.Y;
dz = p.Z + t * pq.Z - pt.Z;
return dx * dx + dy * dy + dz * dz;
}
protected override void ComponentsCreatedHandler(object sender, EventArgs e)
{
base.ComponentsCreatedHandler(sender, e);
TerrainRenderComponent terrainRenderComponent = Owner.GetComponent<TerrainRenderComponent>(ComponentType.Render);
if (terrainRenderComponent == null)
throw new LevelManifestException("TerrainCollisionComponent expect to be accompanied by a TerrainRenderComponent.");
float[,] heightVals = terrainRenderComponent.Heights;
XnaVector3 originShift = new XnaVector3(terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongXAxis - 1) * 0.5f,
0.0f,
terrainRenderComponent.TerrainAsset.XZScale * (terrainRenderComponent.TerrainAsset.VertexCountAlongZAxis - 1) * 0.5f);
AffineTransform terrainTransform = new BEPUutilities.AffineTransform(
new BEPUutilities.Vector3(terrainRenderComponent.TerrainAsset.XZScale, 1.0f, terrainRenderComponent.TerrainAsset.XZScale),
BepuConverter.Convert(mTransformComponent.Orientation),
BepuConverter.Convert(mTransformComponent.Translation - originShift));
mSimTerrain = new BepuTerrain(heightVals, terrainTransform);
mSimTerrain.Material.Bounciness = 0.60f;
mSimTerrain.Material.StaticFriction = 1.0f;
mSimTerrain.Material.KineticFriction = 1.0f;
mSimTerrain.Tag = Owner.Id;
}
// ReSharper restore NotAccessedField.Global
// ReSharper restore MemberCanBePrivate.Global
public VertexPositionTextureNormalBinormalTangent(Vector3 position, Vector2 textureCoordinate, Vector3 normal, Vector3 binormal, Vector3 tangent)
{
Position = position.ToXNA();
TextureCoordinate = textureCoordinate.ToXNA();
Normal = normal.ToXNA();
Binormal = binormal.ToXNA();
Tangent = tangent.ToXNA();
}
/// <summary>
/// Calculates the component-wise maximum of two vectors.
/// </summary>
/// <param name="left">A vector.</param>
/// <param name="right">Another vector.</param>
/// <param name="result">The component-wise maximum of the two vectors.</param>
internal static void ComponentMax(ref Vector3 left, ref Vector3 right, out Vector3 result)
{
#if OPENTK || STANDALONE
Vector3.ComponentMax(ref left, ref right, out result);
#elif UNITY3D
result = Vector3.Min(left, right);
#else
Vector3.Max(ref left, ref right, out result);
#endif
}
/// <summary>
/// Add a new circle to the array
/// </summary>
/// <param name="pos">The position</param>
/// <param name="rad">The radius</param>
/// <param name="vel">The velocity</param>
/// <param name="dvel">The desired velocity</param>
public void AddCircle(Vector3 pos, float rad, Vector3 vel, Vector3 dvel)
{
if (numCircles >= maxCircles)
return;
circles[numCircles].Position = pos;
circles[numCircles].Radius = rad;
circles[numCircles].Vel = vel;
circles[numCircles].DesiredVel = dvel;
numCircles++;
}
internal static void ProjectPoly(Vector3 axis, Vector3[] poly, int npoly, out float rmin, out float rmax)
{
Vector3Extensions.Dot2D(ref axis, ref poly[0], out rmin);
Vector3Extensions.Dot2D(ref axis, ref poly[0], out rmax);
for (int i = 1; i < npoly; i++)
{
float d;
Vector3Extensions.Dot2D(ref axis, ref poly[i], out d);
rmin = Math.Min(rmin, d);
rmax = Math.Max(rmax, d);
}
}
/// <summary>
/// Initializes a new instance of the <see cref="Crowd" /> class.
/// </summary>
/// <param name="maxAgents">The maximum agents allowed</param>
/// <param name="maxAgentRadius">The maximum radius for an agent</param>
/// <param name="navMesh">The navigation mesh</param>
public Crowd(int maxAgents, float maxAgentRadius, ref TiledNavMesh navMesh)
{
this.maxAgents = maxAgents;
this.maxAgentRadius = maxAgentRadius;
this.ext = new Vector3(maxAgentRadius * 2.0f, maxAgentRadius * 1.5f, maxAgentRadius * 2.0f);
//initialize proximity grid
this.grid = new ProximityGrid<Agent>(maxAgents * 4, maxAgentRadius * 3);
//allocate obstacle avoidance query
this.obstacleQuery = new ObstacleAvoidanceQuery(6, 8);
//initialize obstancle query params
this.obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[AgentMaxObstacleAvoidanceParams];
for (int i = 0; i < this.obstacleQueryParams.Length; i++)
{
this.obstacleQueryParams[i].VelBias = 0.4f;
this.obstacleQueryParams[i].WeightDesVel = 2.0f;
this.obstacleQueryParams[i].WeightCurVel = 0.75f;
this.obstacleQueryParams[i].WeightSide = 0.75f;
this.obstacleQueryParams[i].WeightToi = 2.5f;
this.obstacleQueryParams[i].HorizTime = 2.5f;
this.obstacleQueryParams[i].GridSize = 33;
this.obstacleQueryParams[i].AdaptiveDivs = 7;
this.obstacleQueryParams[i].AdaptiveRings = 2;
this.obstacleQueryParams[i].AdaptiveDepth = 5;
}
//allocate temp buffer for merging paths
this.maxPathResult = 256;
this.pathResult = new PolyId[this.maxPathResult];
this.pathq = new PathQueue(maxPathResult, 4096, ref navMesh);
this.agents = new Agent[maxAgents];
this.activeAgents = new Agent[maxAgents];
this.agentAnims = new AgentAnimation[maxAgents];
for (int i = 0; i < maxAgents; i++)
{
this.agents[i] = new Agent(maxPathResult);
}
for (int i = 0; i < maxAgents; i++)
{
this.agentAnims[i].Active = false;
}
//allocate nav mesh query
this.navQuery = new NavMeshQuery(navMesh, 512);
}
public void SegmentSegment2D_without_float_false()
{
//the Segment 1
Vector3 a = new Vector3(0, 0, 0);
Vector3 b = new Vector3(1, 0, 1);
//the segment 2
Vector3 p = new Vector3(1, 0, 0);
Vector3 q = new Vector3(2, 0, 1);
bool f = Intersection.SegmentSegment2D(ref a, ref b, ref p, ref q);
Assert.IsFalse(f);
}
public void PointInPoly_ExternalPoint_Success()
{
Vector3 pt = new Vector3(-1.0f, 0.0f, -1.0f);
Vector3[] poly = new Vector3[3];
poly[0] = new Vector3(0.0f, 0.0f, 1.0f);
poly[1] = new Vector3(-1.0f, 0.0f, 0.0f);
poly[2] = new Vector3(1.0f, 0.0f, 0.0f);
bool isInPoly = Containment.PointInPoly(pt, poly, poly.Length);
Assert.IsFalse(isInPoly);
}
public void PointToTriangle_CenterPointDist_Success()
{
//Point
Vector3 p = new Vector3(0.5f, 0.5f, 0.5f);
//Triangle
Vector3 a = new Vector3(0, 0, 1);
Vector3 b = new Vector3(-1, 0, 0);
Vector3 c = new Vector3(1, 0, 0);
float dist = Distance.PointToTriangle(p, a, b, c);
Assert.AreEqual(dist + float.Epsilon, 0.5f);
}
public void PointToSegment2DSquared_Vectors_Success()
{
//the point
Vector3 pt = new Vector3(0, 0, 0);
//the segment
Vector3 p = new Vector3(0, 0, 1);
Vector3 q = new Vector3(1, 0, 0);
float dist = Distance.PointToSegment2DSquared(ref pt, ref p, ref q);
//safe floating value comparison
Assert.AreEqual(dist + float.Epsilon, 0.5f);
}
/// <summary>
/// Initializes a new instance of the <see cref="TiledNavMesh"/> class.
/// </summary>
/// <param name="data">The Navigation Mesh data</param>
public TiledNavMesh(NavMeshBuilder data)
{
this.origin = data.Header.Bounds.Min;
this.tileWidth = data.Header.Bounds.Max.X - data.Header.Bounds.Min.X;
this.tileHeight = data.Header.Bounds.Max.Z - data.Header.Bounds.Min.Z;
this.maxTiles = 1;
this.maxPolys = data.Header.PolyCount;
if (!InitTileNavMesh())
return;
//HACK is tileRef actually being used for anything?
PolyId tileRef = PolyId.Null;
AddTile(data, PolyId.Null, out tileRef);
}
public void SegmentSegment2D_with_float_success()
{
//the Segment 1
Vector3 a = new Vector3(0, 0, 0);
Vector3 b = new Vector3(1, 0, 1);
//the segment 2
Vector3 p = new Vector3(0, 0, 1);
Vector3 q = new Vector3(1, 0, 0);
float m;
float n;
bool f = Intersection.SegmentSegment2D(ref a, ref b, ref p, ref q, out m, out n);
Assert.IsTrue(f);
}
/// <summary>
/// Iterates over an array of <see cref="Vector3"/> with a specified offset, stride, and length.
/// </summary>
/// <param name="vertices">An array of vertices.</param>
/// <param name="vertOffset">The index of the first vertex to be enumerated.</param>
/// <param name="vertStride">The distance between the start of two triangles. A value of 0 means the data is tightly packed.</param>
/// <param name="triCount">The number of triangles to enumerate.</param>
/// <returns>An enumerable collection of triangles.</returns>
public static IEnumerable<Triangle3> FromVector3(Vector3[] vertices, int vertOffset, int vertStride, int triCount)
{
Triangle3 tri;
if (vertStride == 0)
vertStride = 3;
for (int i = 0; i < triCount; i++)
{
tri.A = vertices[i * vertStride + vertOffset];
tri.B = vertices[i * vertStride + vertOffset + 1];
tri.C = vertices[i * vertStride + vertOffset + 2];
yield return tri;
}
}
/// <summary>
/// Determines whether a point is inside a polygon.
/// </summary>
/// <param name="pt">A point.</param>
/// <param name="verts">A set of vertices that define a polygon.</param>
/// <param name="nverts">The number of vertices to use from <c>verts</c>.</param>
/// <returns>A value indicating whether the point is contained within the polygon.</returns>
internal static bool PointInPoly(Vector3 pt, Vector3[] verts, int nverts)
{
bool c = false;
for (int i = 0, j = nverts - 1; i < nverts; j = i++)
{
Vector3 vi = verts[i];
Vector3 vj = verts[j];
if (((vi.Z > pt.Z) != (vj.Z > pt.Z)) &&
(pt.X < (vj.X - vi.X) * (pt.Z - vi.Z) / (vj.Z - vi.Z) + vi.X))
{
c = !c;
}
}
return c;
}
/// <summary>
/// Determines whether two 2D segments AB and CD are intersecting.
/// </summary>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="c">The endpoint C of segment CD.</param>
/// <param name="d">The endpoint D of segment CD.</param>
/// <returns>A value indicating whether the two segments are intersecting.</returns>
internal static bool SegmentSegment2D(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d)
{
float a1, a2, a3;
Vector3Extensions.Cross2D(ref a, ref b, ref d, out a1);
Vector3Extensions.Cross2D(ref a, ref b, ref c, out a2);
if (a1 * a2 < 0.0f)
{
Vector3Extensions.Cross2D(ref c, ref d, ref a, out a3);
float a4 = a3 + a2 - a1;
if (a3 * a4 < 0.0f)
return true;
}
return false;
}
public override void Initialize(Stage stage)
{
triggered = false;
UsingOnTriggerEnter = true;
UsingOnTriggerStay = false;
UsingOnTriggerExit = false;
rotateRight = true;
rotationTime = 1.0f;
newForwardDir = actor.Parm.GetVector3("NewDir");
newDir = Vector3ToMoveDir(newForwardDir);
if (actor.Parm.HasParm("RotateRight"))
rotateRight = actor.Parm.GetBool("RotateRight");
if (actor.Parm.HasParm("RotationTime"))
rotationTime = actor.Parm.GetFloat("RotationTime");
base.Initialize(stage);
}
public const int VERTS_PER_POLYGON = 6; //max number of vertices
#endregion Fields
#region Methods
/// <summary>
/// Generate an accurate sample of random points in the convex polygon and pick a point.
/// </summary>
/// <param name="pts">The polygon's points data</param>
/// <param name="s">A random float</param>
/// <param name="t">Another random float</param>
/// <param name="pt">The resulting point</param>
public static void RandomPointInConvexPoly(Vector3[] pts, float s, float t, out Vector3 pt)
{
//Calculate triangle areas
float[] areas = new float[pts.Length];
float areaSum = 0.0f;
float area;
for (int i = 2; i < pts.Length; i++)
{
Triangle3.Area2D(ref pts[0], ref pts[i - 1], ref pts[i], out area);
areaSum += Math.Max(0.001f, area);
areas[i] = area;
}
//Find sub triangle weighted by area
float threshold = s * areaSum;
float accumulatedArea = 0.0f;
float u = 0.0f;
int triangleVertex = 0;
for (int i = 2; i < pts.Length; i++)
{
float currentArea = areas[i];
if (threshold >= accumulatedArea && threshold < (accumulatedArea + currentArea))
{
u = (threshold - accumulatedArea) / currentArea;
triangleVertex = i;
break;
}
accumulatedArea += currentArea;
}
float v = (float)Math.Sqrt(t);
float a = 1 - v;
float b = (1 - u) * v;
float c = u * v;
Vector3 pointA = pts[0];
Vector3 pointB = pts[triangleVertex - 1];
Vector3 pointC = pts[triangleVertex];
pt = a * pointA + b * pointB + c * pointC;
}
public BasicEntity(Model model, MaterialEffect material, Vector3 position, double angleZ, double angleX, double angleY, Vector3 scale, MeshMaterialLibrary library = null, Entity physicsObject = null)
{
Id = IdGenerator.GetNewId();
Name = GetType().Name + " " + Id;
WorldTransform = new TransformMatrix(Matrix.Identity, Id);
Model = model;
Material = material;
Position = position;
Scale = scale;
RotationMatrix = Matrix.CreateRotationX((float)angleX) * Matrix.CreateRotationY((float)angleY) *
Matrix.CreateRotationZ((float)angleZ);
if (library != null)
{
RegisterInLibrary(library);
}
if (physicsObject != null)
{
RegisterPhysics(physicsObject);
}
}
public Microsoft.Xna.Framework.Vector3 Solve()
{
Microsoft.Xna.Framework.Vector3 q = qef.Solve2(0, 0, 0);
Vector <float> v = Vector <float> .Build.Dense(new float[] { q.X, q.Y, q.Z });
float error = ComputeTotalPlaneError(v);
Vector <float> result = Vector <float> .Build.Dense(3);
for (int i = 0; i < PlaneNs.Count; i++)
{
//result += ComputePlaneVertex(i);
result += ComputeQEMVertex(i);
}
result /= (float)PlaneNs.Count;
result += GetMassPoint();
error = ComputeTotalPlaneError(result);
if (error != 0)
{
}
return(new Microsoft.Xna.Framework.Vector3(result[0], result[1], result[2]));
}
/// <summary>
/// Request a new move target
/// </summary>
/// <param name="reference">The polygon reference</param>
/// <param name="pos">The target's coordinates</param>
/// <returns>True if request met, false if not</returns>
public bool RequestMoveTarget(NavPolyId reference, Vector3 pos)
{
if (reference == NavPolyId.Null)
{
return(false);
}
//initialize request
this.TargetRef = reference;
this.targetPos = pos;
this.TargetPathQueryIndex = PathQueue.Invalid;
this.TargetReplan = false;
if (this.TargetRef != NavPolyId.Null)
{
this.targetState = TargetState.Requesting;
}
else
{
this.targetState = TargetState.Failed;
}
return(true);
}
/// <summary>
/// Update the position after a certain time 'dt'
/// </summary>
/// <param name="dt">Time that passed</param>
public void Integrate(float dt)
{
//fake dyanmic constraint
float maxDelta = Parameters.MaxAcceleration * dt;
Vector3 dv = NVel - Vel;
float ds = dv.Length();
if (ds > maxDelta)
{
dv = dv * (maxDelta / ds);
}
Vel = Vel + dv;
//integrate
if (Vel.Length() > 0.0001f)
{
currentPos = currentPos + Vel * dt;
}
else
{
Vel = new Vector3(0, 0, 0);
}
}
public void TransformTest()
{
Quaternion[] rotationTestPatterns = { Quaternion.Identity, Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0.5m, 0.5m, 0), 0.72m), Quaternion.CreateFromAxisAngle(new Vector3(0, 0.5m, 0.5m), 0.72m) };
Vector3[] transrationTestPatterns = { Vector3.Zero, Vector3.One, new Vector3(5, 1, 1), new Vector3(1, 5, 1), new Vector3(1, 1, 5), new Vector3(5, 5, 1), new Vector3(5, 1, 5), new Vector3(1, 5, 5), new Vector3(5, 5, 5) };
foreach (var rot in rotationTestPatterns)
{
foreach (var vec in transrationTestPatterns)
{
Vector3 value = vec, result;
Quaternion rotation = rot;
Vector3.Transform(ref value, ref rotation, out result);
Microsoft.Xna.Framework.Vector3 value2 = new Microsoft.Xna.Framework.Vector3((float)vec.X, (float)vec.Y, (float)vec.Z), actual_xna;
Microsoft.Xna.Framework.Quaternion rotation2 = new Microsoft.Xna.Framework.Quaternion((float)rot.X, (float)rot.Y, (float)rot.Z, (float)rot.W);
Microsoft.Xna.Framework.Vector3.Transform(ref value2, ref rotation2, out actual_xna);
Vector3 acutual = new Vector3((decimal)actual_xna.X, (decimal)actual_xna.Y, (decimal)actual_xna.Z);
Assert.IsTrue(Math.Abs(result.X - acutual.X) < 0.001m);
Assert.IsTrue(Math.Abs(result.Y - acutual.Y) < 0.001m);
Assert.IsTrue(Math.Abs(result.Z - acutual.Z) < 0.001m);
}
}
}
public static Vector3 Multiply(this Matrix matrix, Vector3 vector)
{
if (matrix == null)
{
throw new ArgumentNullException("matrix");
}
if (vector == null)
{
throw new ArgumentNullException("vector");
}
var product = new Vector3();
for (var i = 0; i < 3; i++)
{
for (var k = 0; k < 3; k++)
{
product.SetIndex(i, product.Index(i) + matrix[i, k] * vector.Index(k));
}
}
return(product);
}
int SkeletonToLineList(SkeletonAnnotation skeleton, XNAColor color, List <VertexPositionColorTexture> vertexList)
{
if (vertexList == null)
{
return(0);
}
vertexList.Clear();
if (skeleton == null || skeleton.bones == null || skeleton.bones.Count <= 0)
{
return(0);
}
float minx = skeleton.joints.Select(j => j.position.X).Min();
float miny = skeleton.joints.Select(j => j.position.Y).Min();
float maxx = skeleton.joints.Select(j => j.position.X).Max();
float maxy = skeleton.joints.Select(j => j.position.Y).Max();
float ox = (maxx - minx) / 2;
float oy = (maxy - miny) / 2;
for (int i = 0; i < skeleton.bones.Count; i++)
{
if (skeleton.bones[i].src == null || skeleton.bones[i].dst == null)
{
continue;
}
XNAVector3 srcPos = PointFToXNAVector3(skeleton.bones[i].src.position);
XNAVector3 dstPos = PointFToXNAVector3(skeleton.bones[i].dst.position);
vertexList.Add(new VertexPositionColorTexture(srcPos, color, XNAVector2.Zero));
vertexList.Add(new VertexPositionColorTexture(dstPos, color, XNAVector2.Zero));
}
return(vertexList.Count / 2);
}
public List <Microsoft.Xna.Framework.Vector3> DoEdges(List <Microsoft.Xna.Framework.Point> Nodes)
{
List <Geometry.Point> pts = new List <Geometry.Point>();
foreach (Microsoft.Xna.Framework.Point p in Nodes)
{
pts.Add(new Geometry.Point(p.X, p.Y));
}
List <Geometry.Triangle> tris = Triangulate(pts);
List <Microsoft.Xna.Framework.Vector3> Edges = new List <Microsoft.Xna.Framework.Vector3>();
Microsoft.Xna.Framework.Vector2 dist = new Microsoft.Xna.Framework.Vector2();
Microsoft.Xna.Framework.Vector3 curr = new Microsoft.Xna.Framework.Vector3();
foreach (Geometry.Triangle t in tris)
{
dist = Nodes[t.p1].ToVector2() - Nodes[t.p2].ToVector2();
curr = new Microsoft.Xna.Framework.Vector3(t.p1, t.p2, dist.Length());
if (!Edges.Contains(curr))
{
Edges.Add(curr);
}
dist = Nodes[t.p2].ToVector2() - Nodes[t.p3].ToVector2();
curr = new Microsoft.Xna.Framework.Vector3(t.p2, t.p3, dist.Length());
if (!Edges.Contains(curr))
{
Edges.Add(curr);
}
dist = Nodes[t.p3].ToVector2() - Nodes[t.p1].ToVector2();
curr = new Microsoft.Xna.Framework.Vector3(t.p3, t.p1, dist.Length());
if (!Edges.Contains(curr))
{
Edges.Add(curr);
}
}
return(Edges);
}
private void SensorCurrentValueChanged(object sender, SensorReadingEventArgs
<AccelerometerReading> e)
{
// Note that this event handler is called from a background thread
this.Dispatcher.BeginInvoke(() =>
{
Vector3 vec = e.SensorReading.Acceleration;
if (vec.Z < -0.8 && StaticData.mode == 1)
{
//Enable map
UIHelper.ToggleVisibility(overheadMap);
StaticData.mode = 0;
}
else
{
if (vec.Z > -0.8 && StaticData.mode == 0)
{
//Enable Camera
UIHelper.ToggleVisibility(overheadMap);
StaticData.mode = 1;
}
}
});
}
/// <summary>
/// Prepare the obstacles for further calculations
/// </summary>
/// <param name="position">Current position</param>
/// <param name="desiredVel">Desired velocity</param>
public void Prepare(Vector3 position, Vector3 desiredVel)
{
//prepare obstacles
for (int i = 0; i < numCircles; i++)
{
//side
Vector3 pa = position;
Vector3 pb = circles[i].Position;
Vector3 orig = new Vector3(0, 0, 0);
circles[i].Dp = pb - pa;
circles[i].Dp.Normalize();
Vector3 dv = circles[i].DesiredVel - desiredVel;
float a = Triangle3.Area2D(orig, circles[i].Dp, dv);
if (a < 0.01f)
{
circles[i].Np.X = -circles[i].Dp.Z;
circles[i].Np.Z = circles[i].Dp.X;
}
else
{
circles[i].Np.X = circles[i].Dp.Z;
circles[i].Np.Z = -circles[i].Dp.X;
}
}
for (int i = 0; i < numSegments; i++)
{
//precalculate if the agent is close to the segment
float r = 0.01f;
float t;
segments[i].Touch = Distance.PointToSegment2DSquared(ref position, ref segments[i].P,
ref segments[i].Q, out t) < (r * r);
}
}
/// <summary>
/// Determine whether a ray (origin, dir) is intersecting a segment AB.
/// </summary>
/// <param name="origin">The origin of the ray.</param>
/// <param name="dir">The direction of the ray.</param>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="t">The parameter t</param>
/// <returns>A value indicating whether the ray is intersecting with the segment.</returns>
public static bool RaySegment(Vector3 origin, Vector3 dir, Vector3 a, Vector3 b, out float t)
{
//default if not intersectng
t = 0;
Vector3 v = b - a;
Vector3 w = origin - a;
float d;
Vector3Extensions.PerpDotXZ(ref dir, ref v, out d);
d *= -1;
if (Math.Abs(d) < 1e-6f)
{
return(false);
}
d = 1.0f / d;
Vector3Extensions.PerpDotXZ(ref v, ref w, out t);
t *= -d;
if (t < 0 || t > 1)
{
return(false);
}
float s;
Vector3Extensions.PerpDotXZ(ref dir, ref w, out s);
s *= -d;
if (s < 0 || s > 1)
{
return(false);
}
return(true);
}
/// <summary>
/// Determines whether two polygons A and B are intersecting
/// </summary>
/// <param name="polya">Polygon A's vertices</param>
/// <param name="npolya">Number of vertices for polygon A</param>
/// <param name="polyb">Polygon B's vertices</param>
/// <param name="npolyb">Number of vertices for polygon B</param>
/// <returns>True if intersecting, false if not</returns>
internal static bool PolyPoly2D(Vector3[] polya, int npolya, Vector3[] polyb, int npolyb)
{
const float EPS = 1E-4f;
for (int i = 0, j = npolya - 1; i < npolya; j = i++)
{
Vector3 va = polya[j];
Vector3 vb = polya[i];
Vector3 n = new Vector3(va.X - vb.X, 0.0f, va.Z - vb.Z);
float amin, amax, bmin, bmax;
ProjectPoly(n, polya, npolya, out amin, out amax);
ProjectPoly(n, polyb, npolyb, out bmin, out bmax);
if (!OverlapRange(amin, amax, bmin, bmax, EPS))
{
//found separating axis
return(false);
}
}
for (int i = 0, j = npolyb - 1; i < npolyb; j = i++)
{
Vector3 va = polyb[j];
Vector3 vb = polyb[i];
Vector3 n = new Vector3(va.X - vb.X, 0.0f, va.Z - vb.Z);
float amin, amax, bmin, bmax;
ProjectPoly(n, polya, npolya, out amin, out amax);
ProjectPoly(n, polyb, npolyb, out bmin, out bmax);
if (!OverlapRange(amin, amax, bmin, bmax, EPS))
{
//found separating axis
return(false);
}
}
return(true);
}
/// <summary>
/// Determines whether two polygons A and B are intersecting
/// </summary>
/// <param name="polya">Polygon A's vertices</param>
/// <param name="npolya">Number of vertices for polygon A</param>
/// <param name="polyb">Polygon B's vertices</param>
/// <param name="npolyb">Number of vertices for polygon B</param>
/// <returns>True if intersecting, false if not</returns>
internal static bool PolyPoly2D(Vector3[] polya, int npolya, Vector3[] polyb, int npolyb)
{
const float EPS = 1E-4f;
for (int i = 0, j = npolya - 1; i < npolya; j = i++)
{
Vector3 va = polya[j];
Vector3 vb = polya[i];
Vector3 n = new Vector3(va.X - vb.X, 0.0f, va.Z - vb.Z);
float amin, amax, bmin, bmax;
ProjectPoly(n, polya, npolya, out amin, out amax);
ProjectPoly(n, polyb, npolyb, out bmin, out bmax);
if (!OverlapRange(amin, amax, bmin, bmax, EPS))
{
//found separating axis
return false;
}
}
for (int i = 0, j = npolyb - 1; i < npolyb; j = i++)
{
Vector3 va = polyb[j];
Vector3 vb = polyb[i];
Vector3 n = new Vector3(va.X - vb.X, 0.0f, va.Z - vb.Z);
float amin, amax, bmin, bmax;
ProjectPoly(n, polya, npolya, out amin, out amax);
ProjectPoly(n, polyb, npolyb, out bmin, out bmax);
if (!OverlapRange(amin, amax, bmin, bmax, EPS))
{
//found separating axis
return false;
}
}
return true;
}
public static System.Numerics.Vector3 ToNumerics(this Microsoft.Xna.Framework.Vector3 v)
{
return(new System.Numerics.Vector3(v.X, v.Y, v.Z));
}
/// <summary>
/// Initializes a new instance of the <see cref="NavPoint"/> struct.
/// </summary>
/// <param name="poly">The polygon that the point is on.</param>
/// <param name="pos">The 3d position of the point.</param>
public NavPoint(int poly, Vector3 pos)
{
this.Polygon = poly;
this.Position = pos;
}
public void ToVector3(out Microsoft.Xna.Framework.Vector3 result)
{
result = new Microsoft.Xna.Framework.Vector3(X, Y, Z);
}
private static void ApplyPropertiesTo(PositionedObject entity, List <NamedValue> propertiesToAssign, Microsoft.Xna.Framework.Vector3 position)
{
if (entity != null)
{
entity.Position = position;
}
var entityType = entity.GetType();
var lateBinder = Instructions.Reflection.LateBinder.GetInstance(entityType);
foreach (var property in propertiesToAssign)
{
// If name is EntityToCreate, skip it:
string propertyName = property.Name;
bool shouldSet = propertyName != "EntityToCreate";
if (shouldSet)
{
if (propertyName == "name")
{
propertyName = "Name";
}
var valueToSet = property.Value;
var propertyType = property.Type;
if (string.IsNullOrEmpty(propertyType))
{
propertyType = TryGetPropertyType(entityType, propertyName);
}
valueToSet = SetValueAccordingToType(valueToSet, propertyName, propertyType, entityType);
try
{
lateBinder.SetValue(entity, propertyName, valueToSet);
}
catch (InvalidCastException e)
{
string assignedType = valueToSet.GetType().ToString() ?? "unknown type";
assignedType = GetFriendlyNameForType(assignedType);
string expectedType = "unknown type";
object outValue;
if (lateBinder.TryGetValue(entity, propertyName, out outValue) && outValue != null)
{
expectedType = outValue.GetType().ToString();
expectedType = GetFriendlyNameForType(expectedType);
}
// This means that the property exists but is of a different type.
string message = $"Attempted to assign the property {propertyName} " +
$"to a value of type {assignedType} but expected {expectedType}. " +
$"Check the property type in your TMX and make sure it matches the type on the entity.";
throw new Exception(message, e);
}
catch (Exception e)
{
// Since this code indiscriminately tries to set properties, it may set properties which don't
// actually exist. Therefore, we tolerate failures.
}
}
}
}
public MapStatistics(string MapName, Microsoft.Xna.Framework.Point MapSize, Microsoft.Xna.Framework.Point TileSize, Microsoft.Xna.Framework.Vector3 CameraStartPosition,
byte PlayersMin, byte PlayersMax, string Description)
{
InitializeComponent();
txtMapName.Text = MapName;
txtMapWidth.Text = MapSize.X.ToString();
txtMapHeight.Text = MapSize.Y.ToString();
txtTileWidth.Text = TileSize.X.ToString();
txtTileHeight.Text = TileSize.Y.ToString();
txtCameraStartPositionX.Value = (int)CameraStartPosition.X;
txtCameraStartPositionY.Value = (int)CameraStartPosition.Y;
txtPlayersMin.Value = PlayersMin;
txtPlayersMax.Value = PlayersMax;
txtDescription.Text = Description;
ListBackgroundsPath = new List <string>();
ListForegroundsPath = new List <string>();
}
public void ToVector3(out Microsoft.Xna.Framework.Vector3 result)
{
result = new Microsoft.Xna.Framework.Vector3(X, Y, Z);
}
/// <summary>
/// Determines whether two 2D segments AB and CD are intersecting.
/// </summary>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="c">The endpoint C of segment CD.</param>
/// <param name="d">The endpoint D of segment CD.</param>
/// <returns>A value indicating whether the two segments are intersecting.</returns>
internal static bool SegmentSegment2D(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d)
{
float a1, a2, a3;
Vector3Extensions.Cross2D(ref a, ref b, ref d, out a1);
Vector3Extensions.Cross2D(ref a, ref b, ref c, out a2);
if (a1 * a2 < 0.0f)
{
Vector3Extensions.Cross2D(ref c, ref d, ref a, out a3);
float a4 = a3 + a2 - a1;
if (a3 * a4 < 0.0f)
{
return(true);
}
}
return(false);
}
/// <summary>
/// Determines whether the segment interesects with the polygon.
/// </summary>
/// <param name="p0">Segment's first endpoint</param>
/// <param name="p1">Segment's second endpoint</param>
/// <param name="verts">Polygon's vertices</param>
/// <param name="nverts">The number of vertices in the polygon</param>
/// <param name="tmin">Parameter t minimum</param>
/// <param name="tmax">Parameter t maximum</param>
/// <param name="segMin">Minimum vertex index</param>
/// <param name="segMax">Maximum vertex index</param>
/// <returns>True if intersect, false if not</returns>
internal static bool SegmentPoly2D(Vector3 p0, Vector3 p1, Vector3[] verts, int nverts, out float tmin, out float tmax, out int segMin, out int segMax)
{
const float Epsilon = 0.00000001f;
tmin = 0;
tmax = 1;
segMin = -1;
segMax = -1;
Vector3 dir = p1 - p0;
for (int i = 0, j = nverts - 1; i < nverts; j = i++)
{
Vector3 edge = verts[i] - verts[j];
Vector3 diff = p0 - verts[j];
float n = edge.Z * diff.X - edge.X * diff.Z;
float d = dir.Z * edge.X - dir.X * edge.Z;
if (Math.Abs(d) < Epsilon)
{
//S is nearly parallel to this edge
if (n < 0)
{
return(false);
}
else
{
continue;
}
}
float t = n / d;
if (d < 0)
{
//segment S is entering across this edge
if (t > tmin)
{
tmin = t;
segMin = j;
//S enters after leaving the polygon
if (tmin > tmax)
{
return(false);
}
}
}
else
{
//segment S is leaving across this edge
if (t < tmax)
{
tmax = t;
segMax = j;
//S leaves before entering the polygon
if (tmax < tmin)
{
return(false);
}
}
}
}
return(true);
}
public PhysxTriangleMesh(PhysxPhysicWorld PhysxPhysicWorld, FileStream FileStream, Microsoft.Xna.Framework.Matrix localTransformation, Microsoft.Xna.Framework.Matrix worldTransformation, Microsoft.Xna.Framework.Vector3 scale, MaterialDescription MaterialDescription)
{
TriangleMesh triangleMesh = PhysxPhysicWorld.Physix.CreateTriangleMesh(FileStream);
staticActor = PhysxPhysicWorld.Physix.CreateRigidStatic(worldTransformation.AsPhysX());
TriangleMeshGeometry TriangleMeshGeometry = new TriangleMeshGeometry(triangleMesh, new MeshScale(scale.AsPhysX(), Quaternion.Identity));
material = PhysxPhysicWorld.Physix.CreateMaterial(MaterialDescription.StaticFriction, MaterialDescription.DynamicFriction, MaterialDescription.Bounciness);
aTriMeshShape = staticActor.CreateShape(TriangleMeshGeometry, material, localTransformation.AsPhysX());
this.Scale = scale;
}
public PhysxTriangleMesh(PhysxPhysicWorld PhysxPhysicWorld, IModelo model, Microsoft.Xna.Framework.Matrix localTransformation, Microsoft.Xna.Framework.Matrix worldTransformation, Microsoft.Xna.Framework.Vector3 scale, MaterialDescription MaterialDescription)
{
Microsoft.Xna.Framework.Vector3[] vertices = null;
int[] indices = null;
ExtractData(ref vertices, ref indices, model);
TriangleMeshDesc meshDesc = new TriangleMeshDesc();
Vector3[] points = new Vector3[vertices.Count()];
for (int i = 0; i < vertices.Count(); i++)
{
points[i] = vertices[i].AsPhysX();
}
meshDesc.Points = points;
meshDesc.SetTriangles <int>(indices);
//meshDesc.Triangles = indices;
MemoryStream ms = new MemoryStream();
if (PhysxPhysicWorld.Cooking.CookTriangleMesh(meshDesc, ms) == false)
{
PloobsEngine.Engine.Logger.ActiveLogger.LogMessage("Cant Cook Model", Engine.Logger.LogLevel.FatalError);
}
ms.Position = 0;
TriangleMesh triangleMesh = PhysxPhysicWorld.Physix.CreateTriangleMesh(ms);
staticActor = PhysxPhysicWorld.Physix.CreateRigidStatic(worldTransformation.AsPhysX());
TriangleMeshGeometry TriangleMeshGeometry = new TriangleMeshGeometry(triangleMesh, new MeshScale(scale.AsPhysX(), Quaternion.Identity));
material = PhysxPhysicWorld.Physix.CreateMaterial(MaterialDescription.StaticFriction, MaterialDescription.DynamicFriction, MaterialDescription.Bounciness);
aTriMeshShape = staticActor.CreateShape(TriangleMeshGeometry, material, localTransformation.AsPhysX());
this.Scale = scale;
}
public override void ApplyImpulse(Microsoft.Xna.Framework.Vector3 position, Microsoft.Xna.Framework.Vector3 force)
{
ActiveLogger.LogMessage("Cant apply impulse on Triangle Meshes", LogLevel.RecoverableError);
}
private static void ExtractData(ref Microsoft.Xna.Framework.Vector3[] vert, ref int[] ind, IModelo model)
{
List <Microsoft.Xna.Framework.Vector3> vertices = new List <Microsoft.Xna.Framework.Vector3>();
List <int> indices = new List <int>();
for (int i = 0; i < model.MeshNumber; i++)
{
BatchInformation[] bi = model.GetBatchInformation(i);
for (int j = 0; j < bi.Length; j++)
{
BatchInformation info = bi[j];
int offset = vertices.Count;
Microsoft.Xna.Framework.Vector3[] a = new Microsoft.Xna.Framework.Vector3[info.NumVertices];
// Read the format of the vertex buffer
VertexDeclaration declaration = bi[j].VertexBuffer.VertexDeclaration;
VertexElement[] vertexElements = declaration.GetVertexElements();
// Find the element that holds the position
VertexElement vertexPosition = new VertexElement();
foreach (VertexElement elem in vertexElements)
{
if (elem.VertexElementUsage == VertexElementUsage.Position &&
elem.VertexElementFormat == VertexElementFormat.Vector3)
{
vertexPosition = elem;
// There should only be one
break;
}
}
// Check the position element found is valid
if (vertexPosition == null ||
vertexPosition.VertexElementUsage != VertexElementUsage.Position ||
vertexPosition.VertexElementFormat != VertexElementFormat.Vector3)
{
throw new Exception("Model uses unsupported vertex format!");
}
// This where we store the vertices until transformed
// Read the vertices from the buffer in to the array
bi[j].VertexBuffer.GetData <Microsoft.Xna.Framework.Vector3>(
bi[j].BaseVertex * declaration.VertexStride + vertexPosition.Offset,
a,
0,
bi[j].NumVertices,
declaration.VertexStride);
for (int k = 0; k != a.Length; ++k)
{
Microsoft.Xna.Framework.Vector3.Transform(ref a[k], ref info.ModelLocalTransformation, out a[k]);
}
vertices.AddRange(a);
if (info.IndexBuffer.IndexElementSize != IndexElementSize.SixteenBits)
{
int[] s = new int[info.PrimitiveCount * 3];
info.IndexBuffer.GetData <int>(info.StartIndex * 2, s, 0, info.PrimitiveCount * 3);
for (int k = 0; k != info.PrimitiveCount; ++k)
{
indices.Add(s[k * 3 + 2] + offset);
indices.Add(s[k * 3 + 1] + offset);
indices.Add(s[k * 3 + 0] + offset);
}
}
else
{
short[] s = new short[info.PrimitiveCount * 3];
info.IndexBuffer.GetData <short>(info.StartIndex * 2, s, 0, info.PrimitiveCount * 3);
for (int k = 0; k != info.PrimitiveCount; ++k)
{
indices.Add(s[k * 3 + 2] + offset);
indices.Add(s[k * 3 + 1] + offset);
indices.Add(s[k * 3 + 0] + offset);
}
}
}
}
ind = indices.ToArray();
vert = vertices.ToArray();
}
public static Vector3 FromXna(this Microsoft.Xna.Framework.Vector3 v)
{
return(new Vector3(v.X, v.Y, v.Z));
}
public static Symphony.Vector3 ToSymphonyVector3(this Microsoft.Xna.Framework.Vector3 self)
{
return(new Symphony.Vector3(self.X, self.Y, self.Z));
}
/// <summary>
/// Determines whether two 2D segments AB and CD are intersecting.
/// </summary>
/// <param name="a">The endpoint A of segment AB.</param>
/// <param name="b">The endpoint B of segment AB.</param>
/// <param name="c">The endpoint C of segment CD.</param>
/// <param name="d">The endpoint D of segment CD.</param>
/// <param name="s">The normalized dot product between CD and AC on the XZ plane.</param>
/// <param name="t">The normalized dot product between AB and AC on the XZ plane.</param>
/// <returns>A value indicating whether the two segments are intersecting.</returns>
internal static bool SegmentSegment2D(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, out float s, out float t)
{
Vector3 u = b - a;
Vector3 v = d - c;
Vector3 w = a - c;
float magnitude;
Vector3Extensions.PerpDotXZ(ref u, ref v, out magnitude);
if (Math.Abs(magnitude) < 1e-6f)
{
//TODO is NaN the best value to set here?
s = float.NaN;
t = float.NaN;
return(false);
}
Vector3Extensions.PerpDotXZ(ref v, ref w, out s);
Vector3Extensions.PerpDotXZ(ref u, ref w, out t);
s /= magnitude;
t /= magnitude;
return(true);
}
/// <summary>
/// Gets the boundary side of a point relative to a bounding box.
/// </summary>
/// <param name="pt">A point.</param>
/// <param name="bounds">A bounding box.</param>
/// <returns>The point's position relative to the bounding box.</returns>
public static BoundarySide FromPoint(Vector3 pt, BBox3 bounds)
{
const int PlusX = 0x1;
const int PlusZ = 0x2;
const int MinusX = 0x4;
const int MinusZ = 0x8;
int outcode = 0;
outcode |= (pt.X >= bounds.Max.X) ? PlusX : 0;
outcode |= (pt.Z >= bounds.Max.Z) ? PlusZ : 0;
outcode |= (pt.X < bounds.Min.X) ? MinusX : 0;
outcode |= (pt.Z < bounds.Min.Z) ? MinusZ : 0;
switch (outcode)
{
case PlusX:
return BoundarySide.PlusX;
case PlusX | PlusZ:
return BoundarySide.PlusXPlusZ;
case PlusZ:
return BoundarySide.PlusZ;
case MinusX | PlusZ:
return BoundarySide.MinusXPlusZ;
case MinusX:
return BoundarySide.MinusX;
case MinusX | MinusZ:
return BoundarySide.MinusXMinusZ;
case MinusZ:
return BoundarySide.MinusZ;
case PlusX | MinusZ:
return BoundarySide.PlusXMinusZ;
default:
return BoundarySide.Internal;
}
}
/// <summary>
/// Initializes a new instance of the <see cref="NavMeshBuilder" /> class.
/// Add all the PolyMesh and PolyMeshDetail attributes to the Navigation Mesh.
/// Then, add Off-Mesh connection support.
/// </summary>
/// <param name="polyMesh">The PolyMesh</param>
/// <param name="polyMeshDetail">The PolyMeshDetail</param>
/// <param name="offMeshCons">Offmesh connection data</param>
/// <param name="settings">The settings used to build.</param>
public NavMeshBuilder(PolyMesh polyMesh, PolyMeshDetail polyMeshDetail, OffMeshConnection[] offMeshCons, NavMeshGenerationSettings settings)
{
if (settings.VertsPerPoly > PathfindingCommon.VERTS_PER_POLYGON)
{
throw new InvalidOperationException("The number of vertices per polygon is above SharpNav's limit");
}
if (polyMesh.VertCount == 0)
{
throw new InvalidOperationException("The provided PolyMesh has no vertices.");
}
if (polyMesh.PolyCount == 0)
{
throw new InvalidOperationException("The provided PolyMesh has not polys.");
}
int nvp = settings.VertsPerPoly;
//classify off-mesh connection points
BoundarySide[] offMeshSides = new BoundarySide[offMeshCons.Length * 2];
int storedOffMeshConCount = 0;
int offMeshConLinkCount = 0;
if (offMeshCons.Length > 0)
{
//find height bounds
float hmin = float.MaxValue;
float hmax = -float.MaxValue;
if (polyMeshDetail != null)
{
for (int i = 0; i < polyMeshDetail.VertCount; i++)
{
float h = polyMeshDetail.Verts[i].Y;
hmin = Math.Min(hmin, h);
hmax = Math.Max(hmax, h);
}
}
else
{
for (int i = 0; i < polyMesh.VertCount; i++)
{
PolyVertex iv = polyMesh.Verts[i];
float h = polyMesh.Bounds.Min.Y + iv.Y * settings.CellHeight;
hmin = Math.Min(hmin, h);
hmax = Math.Max(hmax, h);
}
}
hmin -= settings.MaxClimb;
hmax += settings.MaxClimb;
BBox3 bounds = polyMesh.Bounds;
bounds.Min.Y = hmin;
bounds.Max.Y = hmax;
for (int i = 0; i < offMeshCons.Length; i++)
{
Vector3 p0 = offMeshCons[i].Pos0;
Vector3 p1 = offMeshCons[i].Pos1;
offMeshSides[i * 2 + 0] = BoundarySideExtensions.FromPoint(p0, bounds);
offMeshSides[i * 2 + 1] = BoundarySideExtensions.FromPoint(p1, bounds);
//off-mesh start position isn't touching mesh
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
if (p0.Y < bounds.Min.Y || p0.Y > bounds.Max.Y)
{
offMeshSides[i * 2 + 0] = 0;
}
}
//count number of links to allocate
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
offMeshConLinkCount++;
}
if (offMeshSides[i * 2 + 1] == BoundarySide.Internal)
{
offMeshConLinkCount++;
}
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
storedOffMeshConCount++;
}
}
}
//off-mesh connections stored as polygons, adjust values
int totPolyCount = polyMesh.PolyCount + storedOffMeshConCount;
int totVertCount = polyMesh.VertCount + storedOffMeshConCount * 2;
//find portal edges
int edgeCount = 0;
int portalCount = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
PolyMesh.Polygon p = polyMesh.Polys[i];
for (int j = 0; j < nvp; j++)
{
if (p.Vertices[j] == PolyMesh.NullId)
{
break;
}
edgeCount++;
if (PolyMesh.IsBoundaryEdge(p.NeighborEdges[j]))
{
int dir = p.NeighborEdges[j] % 16;
if (dir != 15)
{
portalCount++;
}
}
}
}
int maxLinkCount = edgeCount + portalCount * 2 + offMeshConLinkCount * 2;
//find unique detail vertices
int uniqueDetailVertCount = 0;
int detailTriCount = 0;
if (polyMeshDetail != null)
{
detailTriCount = polyMeshDetail.TrisCount;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numDetailVerts = polyMeshDetail.Meshes[i].VertexCount;
int numPolyVerts = polyMesh.Polys[i].VertexCount;
uniqueDetailVertCount += numDetailVerts - numPolyVerts;
}
}
else
{
uniqueDetailVertCount = 0;
detailTriCount = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numPolyVerts = polyMesh.Polys[i].VertexCount;
uniqueDetailVertCount += numPolyVerts - 2;
}
}
//allocate data
header = new PathfindingCommon.NavMeshInfo();
navVerts = new Vector3[totVertCount];
navPolys = new Poly[totPolyCount];
navDMeshes = new PolyMeshDetail.MeshData[polyMesh.PolyCount];
navDVerts = new Vector3[uniqueDetailVertCount];
navDTris = new PolyMeshDetail.TriangleData[detailTriCount];
offMeshConnections = new OffMeshConnection[storedOffMeshConCount];
//store header
//HACK TiledNavMesh should figure out the X/Y/layer instead of the user maybe?
header.X = 0;
header.Y = 0;
header.Layer = 0;
header.PolyCount = totPolyCount;
header.VertCount = totVertCount;
header.MaxLinkCount = maxLinkCount;
header.Bounds = polyMesh.Bounds;
header.DetailMeshCount = polyMesh.PolyCount;
header.DetailVertCount = uniqueDetailVertCount;
header.DetailTriCount = detailTriCount;
header.OffMeshBase = polyMesh.PolyCount;
header.WalkableHeight = settings.AgentHeight;
header.WalkableRadius = settings.AgentRadius;
header.WalkableClimb = settings.MaxClimb;
header.OffMeshConCount = storedOffMeshConCount;
header.BvNodeCount = settings.BuildBoundingVolumeTree ? polyMesh.PolyCount * 2 : 0;
header.BvQuantFactor = 1f / settings.CellSize;
int offMeshVertsBase = polyMesh.VertCount;
int offMeshPolyBase = polyMesh.PolyCount;
//store vertices
for (int i = 0; i < polyMesh.VertCount; i++)
{
PolyVertex iv = polyMesh.Verts[i];
navVerts[i].X = polyMesh.Bounds.Min.X + iv.X * settings.CellSize;
navVerts[i].Y = polyMesh.Bounds.Min.Y + iv.Y * settings.CellHeight;
navVerts[i].Z = polyMesh.Bounds.Min.Z + iv.Z * settings.CellSize;
}
//off-mesh link vertices
int n = 0;
for (int i = 0; i < offMeshCons.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
navVerts[offMeshVertsBase + (n * 2 + 0)] = offMeshCons[i].Pos0;
navVerts[offMeshVertsBase + (n * 2 + 1)] = offMeshCons[i].Pos1;
n++;
}
}
//store polygons
for (int i = 0; i < polyMesh.PolyCount; i++)
{
navPolys[i] = new Poly();
navPolys[i].VertCount = 0;
navPolys[i].Tag = polyMesh.Polys[i].Tag;
navPolys[i].Area = polyMesh.Polys[i].Area;
navPolys[i].PolyType = PolygonType.Ground;
navPolys[i].Verts = new int[nvp];
navPolys[i].Neis = new int[nvp];
for (int j = 0; j < nvp; j++)
{
if (polyMesh.Polys[i].Vertices[j] == PolyMesh.NullId)
{
break;
}
navPolys[i].Verts[j] = polyMesh.Polys[i].Vertices[j];
if (PolyMesh.IsBoundaryEdge(polyMesh.Polys[i].NeighborEdges[j]))
{
//border or portal edge
int dir = polyMesh.Polys[i].NeighborEdges[j] % 16;
if (dir == 0xf) //border
{
navPolys[i].Neis[j] = 0;
}
else if (dir == 0) //portal x-
{
navPolys[i].Neis[j] = Link.External | 4;
}
else if (dir == 1) //portal z+
{
navPolys[i].Neis[j] = Link.External | 2;
}
else if (dir == 2) //portal x+
{
navPolys[i].Neis[j] = Link.External | 0;
}
else if (dir == 3) //portal z-
{
navPolys[i].Neis[j] = Link.External | 6;
}
}
else
{
//normal connection
navPolys[i].Neis[j] = polyMesh.Polys[i].NeighborEdges[j] + 1;
}
navPolys[i].VertCount++;
}
}
//off-mesh connection vertices
n = 0;
for (int i = 0; i < offMeshCons.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
navPolys[offMeshPolyBase + n].VertCount = 2;
navPolys[offMeshPolyBase + n].Verts = new int[nvp];
navPolys[offMeshPolyBase + n].Verts[0] = offMeshVertsBase + (n * 2 + 0);
navPolys[offMeshPolyBase + n].Verts[1] = offMeshVertsBase + (n * 2 + 1);
navPolys[offMeshPolyBase + n].Tag = offMeshCons[i].Flags;
navPolys[offMeshPolyBase + n].Area = polyMesh.Polys[offMeshCons[i].Poly].Area; //HACK is this correct?
navPolys[offMeshPolyBase + n].PolyType = PolygonType.OffMeshConnection;
n++;
}
}
//store detail meshes and vertices
if (polyMeshDetail != null)
{
int vbase = 0;
List <Vector3> storedDetailVerts = new List <Vector3>();
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int vb = polyMeshDetail.Meshes[i].VertexIndex;
int numDetailVerts = polyMeshDetail.Meshes[i].VertexCount;
int numPolyVerts = navPolys[i].VertCount;
navDMeshes[i].VertexIndex = vbase;
navDMeshes[i].VertexCount = numDetailVerts - numPolyVerts;
navDMeshes[i].TriangleIndex = polyMeshDetail.Meshes[i].TriangleIndex;
navDMeshes[i].TriangleCount = polyMeshDetail.Meshes[i].TriangleCount;
//Copy detail vertices
//first 'nv' verts are equal to nav poly verts
//the rest are detail verts
for (int j = 0; j < navDMeshes[i].VertexCount; j++)
{
storedDetailVerts.Add(polyMeshDetail.Verts[vb + numPolyVerts + j]);
}
vbase += numDetailVerts - numPolyVerts;
}
navDVerts = storedDetailVerts.ToArray();
//store triangles
for (int j = 0; j < polyMeshDetail.TrisCount; j++)
{
navDTris[j] = polyMeshDetail.Tris[j];
}
}
else
{
//create dummy detail mesh by triangulating polys
int tbase = 0;
for (int i = 0; i < polyMesh.PolyCount; i++)
{
int numPolyVerts = navPolys[i].VertCount;
navDMeshes[i].VertexIndex = 0;
navDMeshes[i].VertexCount = 0;
navDMeshes[i].TriangleIndex = tbase;
navDMeshes[i].TriangleCount = numPolyVerts - 2;
//triangulate polygon
for (int j = 2; j < numPolyVerts; j++)
{
navDTris[tbase].VertexHash0 = 0;
navDTris[tbase].VertexHash1 = j - 1;
navDTris[tbase].VertexHash2 = j;
//bit for each edge that belongs to the poly boundary
navDTris[tbase].Flags = 1 << 2;
if (j == 2)
{
navDTris[tbase].Flags |= 1 << 0;
}
if (j == numPolyVerts - 1)
{
navDTris[tbase].Flags |= 1 << 4;
}
tbase++;
}
}
}
//store and create BV tree
if (settings.BuildBoundingVolumeTree)
{
//build tree
navBvTree = new BVTree(polyMesh.Verts, polyMesh.Polys, nvp, settings.CellSize, settings.CellHeight);
}
//store off-mesh connections
n = 0;
for (int i = 0; i < offMeshConnections.Length; i++)
{
//only store connections which start from this tile
if (offMeshSides[i * 2 + 0] == BoundarySide.Internal)
{
offMeshConnections[n].Poly = offMeshPolyBase + n;
//copy connection end points
offMeshConnections[n].Pos0 = offMeshCons[i].Pos0;
offMeshConnections[n].Pos1 = offMeshCons[i].Pos1;
offMeshConnections[n].Radius = offMeshCons[i].Radius;
offMeshConnections[n].Flags = offMeshCons[i].Flags;
offMeshConnections[n].Side = offMeshSides[i * 2 + 1];
offMeshConnections[n].Tag = offMeshCons[i].Tag;
n++;
}
}
}
/// <summary>
/// Reset all the internal data
/// </summary>
public void Reset()
{
center = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);
segCount = 0;
numPolys = 0;
}
public static float Dot(Microsoft.Xna.Framework.Vector3 a, IndexedVector3 b)
{
return((a.X * b.X) + (a.Y * b.Y) + (a.Z * b.Z));
}
/// <summary>
/// Ases the vector3.
/// </summary>
/// <param name="reading">The reading.</param>
/// <returns>Vector3.</returns>
public static Vector3 AsVector3(this Microsoft.Xna.Framework.Vector3 reading)
{
return(new Vector3(reading.X, reading.Y, reading.Z));
}
public IndexedVector3(Microsoft.Xna.Framework.Vector3 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
}
