remove vCopy(ref Vector3, Vector3)

This commit is contained in:
ikpil 2023-04-12 23:53:28 +09:00
parent b1b2bb9e3e
commit dfafb03103
19 changed files with 84 additions and 91 deletions

View File

@ -222,13 +222,6 @@ namespace DotRecast.Core
@out.z = @in[2]; @out.z = @in[2];
} }
public static void vCopy(ref Vector3f @out, Vector3f @in)
{
@out.x = @in[0];
@out.y = @in[1];
@out.z = @in[2];
}
public static void vCopy(ref Vector3f @out, float[] @in, int i) public static void vCopy(ref Vector3f @out, float[] @in, int i)
{ {
@out.x = @in[i]; @out.x = @in[i];

View File

@ -257,7 +257,7 @@ namespace DotRecast.Detour.Crowd
ag.dvel = Vector3f.Zero; ag.dvel = Vector3f.Zero;
ag.nvel = Vector3f.Zero; ag.nvel = Vector3f.Zero;
ag.vel = Vector3f.Zero; ag.vel = Vector3f.Zero;
vCopy(ref ag.npos, nearest); ag.npos = nearest;
ag.desiredSpeed = 0; ag.desiredSpeed = 0;
@ -325,7 +325,7 @@ namespace DotRecast.Detour.Crowd
{ {
// Initialize request. // Initialize request.
agent.targetRef = 0; agent.targetRef = 0;
vCopy(ref agent.targetPos, vel); agent.targetPos = vel;
agent.targetPathQueryResult = null; agent.targetPathQueryResult = null;
agent.targetReplan = false; agent.targetReplan = false;
agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY; agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY;
@ -455,7 +455,7 @@ namespace DotRecast.Detour.Crowd
// First check that the current location is valid. // First check that the current location is valid.
Vector3f agentPos = new Vector3f(); Vector3f agentPos = new Vector3f();
long agentRef = ag.corridor.getFirstPoly(); long agentRef = ag.corridor.getFirstPoly();
vCopy(ref agentPos, ag.npos); agentPos = ag.npos;
if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType])) if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType]))
{ {
// Current location is not valid, try to reposition. // Current location is not valid, try to reposition.
@ -465,7 +465,7 @@ namespace DotRecast.Detour.Crowd
agentRef = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L; agentRef = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L;
if (nearestPoly.succeeded()) if (nearestPoly.succeeded())
{ {
vCopy(ref agentPos, nearestPoly.result.getNearestPos()); agentPos = nearestPoly.result.getNearestPos();
} }
if (agentRef == 0) if (agentRef == 0)
@ -485,7 +485,7 @@ namespace DotRecast.Detour.Crowd
// ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery, // ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery,
// &m_filter); // &m_filter);
ag.boundary.reset(); ag.boundary.reset();
vCopy(ref ag.npos, agentPos); ag.npos = agentPos;
replan = true; replan = true;
} }
@ -510,7 +510,7 @@ namespace DotRecast.Detour.Crowd
ag.targetRef = fnp.succeeded() ? fnp.result.getNearestRef() : 0L; ag.targetRef = fnp.succeeded() ? fnp.result.getNearestRef() : 0L;
if (fnp.succeeded()) if (fnp.succeeded())
{ {
vCopy(ref ag.targetPos, fnp.result.getNearestPos()); ag.targetPos = fnp.result.getNearestPos();
} }
replan = true; replan = true;
@ -626,14 +626,14 @@ namespace DotRecast.Detour.Crowd
} }
else else
{ {
vCopy(ref reqPos, ag.targetPos); reqPos = ag.targetPos;
} }
} }
else else
{ {
// Could not find path, start the request from current // Could not find path, start the request from current
// location. // location.
vCopy(ref reqPos, ag.npos); reqPos = ag.npos;
reqPath = new List<long>(); reqPath = new List<long>();
reqPath.Add(path[0]); reqPath.Add(path[0]);
} }
@ -965,8 +965,8 @@ namespace DotRecast.Detour.Crowd
// Copy data for debug purposes. // Copy data for debug purposes.
if (debugAgent == ag) if (debugAgent == ag)
{ {
vCopy(ref debug.optStart, ag.corridor.getPos()); debug.optStart = ag.corridor.getPos();
vCopy(ref debug.optEnd, target); debug.optEnd = target;
} }
} }
else else
@ -1011,7 +1011,7 @@ namespace DotRecast.Detour.Crowd
if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, ref anim.startPos, if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, ref anim.startPos,
ref anim.endPos, navQuery)) ref anim.endPos, navQuery))
{ {
vCopy(ref anim.initPos, ag.npos); anim.initPos = ag.npos;
anim.polyRef = refs[1]; anim.polyRef = refs[1];
anim.active = true; anim.active = true;
anim.t = 0.0f; anim.t = 0.0f;
@ -1051,7 +1051,7 @@ namespace DotRecast.Detour.Crowd
if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY)
{ {
vCopy(ref dvel, ag.targetPos); dvel = ag.targetPos;
ag.desiredSpeed = vLen(ag.targetPos); ag.desiredSpeed = vLen(ag.targetPos);
} }
else else
@ -1124,7 +1124,7 @@ namespace DotRecast.Detour.Crowd
} }
// Set the desired velocity. // Set the desired velocity.
vCopy(ref ag.dvel, dvel); ag.dvel = dvel;
} }
_telemetry.stop("calculateSteering"); _telemetry.stop("calculateSteering");
@ -1198,7 +1198,7 @@ namespace DotRecast.Detour.Crowd
else else
{ {
// If not using velocity planning, new velocity is directly the desired velocity. // If not using velocity planning, new velocity is directly the desired velocity.
vCopy(ref ag.nvel, ag.dvel); ag.nvel = ag.dvel;
} }
} }
@ -1311,7 +1311,7 @@ namespace DotRecast.Detour.Crowd
// Move along navmesh. // Move along navmesh.
ag.corridor.movePosition(ag.npos, navQuery, m_filters[ag.option.queryFilterType]); ag.corridor.movePosition(ag.npos, navQuery, m_filters[ag.option.queryFilterType]);
// Get valid constrained position back. // Get valid constrained position back.
vCopy(ref ag.npos, ag.corridor.getPos()); ag.npos = ag.corridor.getPos();
// If not using path, truncate the corridor to just one poly. // If not using path, truncate the corridor to just one poly.
if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE

View File

@ -229,7 +229,7 @@ namespace DotRecast.Detour.Crowd
public void setTarget(long refs, Vector3f pos) public void setTarget(long refs, Vector3f pos)
{ {
targetRef = refs; targetRef = refs;
vCopy(ref targetPos, pos); targetPos = pos;
targetPathQueryResult = null; targetPathQueryResult = null;
if (targetRef != 0) if (targetRef != 0)
{ {

View File

@ -109,7 +109,7 @@ namespace DotRecast.Detour.Crowd
return; return;
} }
vCopy(ref m_center, pos); m_center = pos;
// First query non-overlapping polygons. // First query non-overlapping polygons.
Result<FindLocalNeighbourhoodResult> res = navquery.findLocalNeighbourhood(refs, pos, collisionQueryRange, Result<FindLocalNeighbourhoodResult> res = navquery.findLocalNeighbourhood(refs, pos, collisionQueryRange,
filter); filter);

View File

@ -159,10 +159,10 @@ namespace DotRecast.Detour.Crowd
return; return;
ObstacleCircle cir = m_circles[m_ncircles++]; ObstacleCircle cir = m_circles[m_ncircles++];
vCopy(ref cir.p, pos); cir.p = pos;
cir.rad = rad; cir.rad = rad;
vCopy(ref cir.vel, vel); cir.vel = vel;
vCopy(ref cir.dvel, dvel); cir.dvel = dvel;
} }
public void addSegment(Vector3f p, Vector3f q) public void addSegment(Vector3f p, Vector3f q)
@ -171,8 +171,8 @@ namespace DotRecast.Detour.Crowd
return; return;
ObstacleSegment seg = m_segments[m_nsegments++]; ObstacleSegment seg = m_segments[m_nsegments++];
vCopy(ref seg.p, p); seg.p = p;
vCopy(ref seg.q, q); seg.q = q;
} }
public int getObstacleCircleCount() public int getObstacleCircleCount()
@ -208,7 +208,7 @@ namespace DotRecast.Detour.Crowd
Vector3f orig = new Vector3f(); Vector3f orig = new Vector3f();
Vector3f dv = new Vector3f(); Vector3f dv = new Vector3f();
vCopy(ref cir.dp, vSub(pb, pa)); cir.dp = vSub(pb, pa);
vNormalize(ref cir.dp); vNormalize(ref cir.dp);
dv = vSub(cir.dvel, dvel); dv = vSub(cir.dvel, dvel);
@ -429,7 +429,7 @@ namespace DotRecast.Detour.Crowd
if (penalty < minPenalty) if (penalty < minPenalty)
{ {
minPenalty = penalty; minPenalty = penalty;
vCopy(ref nvel, vcand); nvel = vcand;
} }
} }
} }
@ -558,16 +558,16 @@ namespace DotRecast.Detour.Crowd
if (penalty < minPenalty) if (penalty < minPenalty)
{ {
minPenalty = penalty; minPenalty = penalty;
vCopy(ref bvel, vcand); bvel = vcand;
} }
} }
vCopy(ref res, bvel); res = bvel;
cr *= 0.5f; cr *= 0.5f;
} }
vCopy(ref nvel, res); nvel = res;
return Tuple.Create(ns, nvel); return Tuple.Create(ns, nvel);
} }

View File

@ -210,8 +210,8 @@ namespace DotRecast.Detour.Crowd
{ {
m_path.Clear(); m_path.Clear();
m_path.Add(refs); m_path.Add(refs);
vCopy(ref m_pos, pos); m_pos = pos;
vCopy(ref m_target, pos); m_target = pos;
} }
private static readonly float MIN_TARGET_DIST = sqr(0.01f); private static readonly float MIN_TARGET_DIST = sqr(0.01f);
@ -390,9 +390,9 @@ namespace DotRecast.Detour.Crowd
var startEnd = nav.getOffMeshConnectionPolyEndPoints(refs[0], refs[1]); var startEnd = nav.getOffMeshConnectionPolyEndPoints(refs[0], refs[1]);
if (startEnd.succeeded()) if (startEnd.succeeded())
{ {
vCopy(ref m_pos, startEnd.result.Item2); m_pos = startEnd.result.Item2;
vCopy(ref start, startEnd.result.Item1); start = startEnd.result.Item1;
vCopy(ref end, startEnd.result.Item2); end = startEnd.result.Item2;
return true; return true;
} }
@ -430,7 +430,7 @@ namespace DotRecast.Detour.Crowd
{ {
m_path = mergeCorridorStartMoved(m_path, masResult.result.getVisited()); m_path = mergeCorridorStartMoved(m_path, masResult.result.getVisited());
// Adjust the position to stay on top of the navmesh. // Adjust the position to stay on top of the navmesh.
vCopy(ref m_pos, masResult.result.getResultPos()); m_pos = masResult.result.getResultPos();
Result<float> hr = navquery.getPolyHeight(m_path[0], masResult.result.getResultPos()); Result<float> hr = navquery.getPolyHeight(m_path[0], masResult.result.getResultPos());
if (hr.succeeded()) if (hr.succeeded())
{ {
@ -473,7 +473,7 @@ namespace DotRecast.Detour.Crowd
* float h = m_target[1]; navquery->getPolyHeight(m_path[m_npath-1], * float h = m_target[1]; navquery->getPolyHeight(m_path[m_npath-1],
* result, &h); result[1] = h; * result, &h); result[1] = h;
*/ */
vCopy(ref m_target, masResult.result.getResultPos()); m_target = masResult.result.getResultPos();
return true; return true;
} }
@ -492,13 +492,13 @@ namespace DotRecast.Detour.Crowd
*/ */
public void setCorridor(Vector3f target, List<long> path) public void setCorridor(Vector3f target, List<long> path)
{ {
vCopy(ref m_target, target); m_target = target;
m_path = new List<long>(path); m_path = new List<long>(path);
} }
public void fixPathStart(long safeRef, Vector3f safePos) public void fixPathStart(long safeRef, Vector3f safePos)
{ {
vCopy(ref m_pos, safePos); m_pos = safePos;
if (m_path.Count < 3 && m_path.Count > 0) if (m_path.Count < 3 && m_path.Count > 0)
{ {
long p = m_path[m_path.Count - 1]; long p = m_path[m_path.Count - 1];
@ -541,7 +541,7 @@ namespace DotRecast.Detour.Crowd
var result = navquery.closestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target); var result = navquery.closestPointOnPolyBoundary(m_path[m_path.Count - 1], m_target);
if (result.succeeded()) if (result.succeeded())
{ {
vCopy(ref m_target, result.result); m_target = result.result;
} }
} }

View File

@ -85,9 +85,9 @@ namespace DotRecast.Detour.Crowd
} }
PathQuery q = new PathQuery(); PathQuery q = new PathQuery();
vCopy(ref q.startPos, startPos); q.startPos = startPos;
q.startRef = startRef; q.startRef = startRef;
vCopy(ref q.endPos, endPos); q.endPos = endPos;
q.endRef = endRef; q.endRef = endRef;
q.result.status = null; q.result.status = null;
q.filter = filter; q.filter = filter;

View File

@ -17,7 +17,7 @@ namespace DotRecast.Detour.Extras.Jumplink
public EdgeSampler(Edge edge, Trajectory trajectory) public EdgeSampler(Edge edge, Trajectory trajectory)
{ {
this.trajectory = trajectory; this.trajectory = trajectory;
vCopy(ref ax, vSub(edge.sq, edge.sp)); ax = vSub(edge.sq, edge.sp);
vNormalize(ref ax); vNormalize(ref ax);
vSet(ref az, ax[2], 0, -ax[0]); vSet(ref az, ax[2], 0, -ax[0]);
vNormalize(ref az); vNormalize(ref az);

View File

@ -363,7 +363,7 @@ namespace DotRecast.Detour.TileCache
TileCacheObstacle ob = allocObstacle(); TileCacheObstacle ob = allocObstacle();
ob.type = TileCacheObstacle.TileCacheObstacleType.CYLINDER; ob.type = TileCacheObstacle.TileCacheObstacleType.CYLINDER;
vCopy(ref ob.pos, pos); ob.pos = pos;
ob.radius = radius; ob.radius = radius;
ob.height = height; ob.height = height;
@ -387,8 +387,8 @@ namespace DotRecast.Detour.TileCache
{ {
TileCacheObstacle ob = allocObstacle(); TileCacheObstacle ob = allocObstacle();
ob.type = TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX; ob.type = TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX;
vCopy(ref ob.center, center); ob.center = center;
vCopy(ref ob.extents, extents); ob.extents = extents;
float coshalf = (float)Math.Cos(0.5f * yRadians); float coshalf = (float)Math.Cos(0.5f * yRadians);
float sinhalf = (float)Math.Sin(-0.5f * yRadians); float sinhalf = (float)Math.Sin(-0.5f * yRadians);
ob.rotAux[0] = coshalf * sinhalf; ob.rotAux[0] = coshalf * sinhalf;
@ -706,8 +706,8 @@ namespace DotRecast.Detour.TileCache
} }
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.BOX) else if (ob.type == TileCacheObstacle.TileCacheObstacleType.BOX)
{ {
vCopy(ref bmin, ob.bmin); bmin = ob.bmin;
vCopy(ref bmax, ob.bmax); bmax = ob.bmax;
} }
else if (ob.type == TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX) else if (ob.type == TileCacheObstacle.TileCacheObstacleType.ORIENTED_BOX)
{ {

View File

@ -270,43 +270,43 @@ namespace DotRecast.Detour
{ {
if (between(a, b, c) && between(a, b, d)) if (between(a, b, c) && between(a, b, d))
{ {
vCopy(ref p, c); p = c;
vCopy(ref q, d); q = d;
return Intersection.Overlap; return Intersection.Overlap;
} }
if (between(c, d, a) && between(c, d, b)) if (between(c, d, a) && between(c, d, b))
{ {
vCopy(ref p, a); p = a;
vCopy(ref q, b); q = b;
return Intersection.Overlap; return Intersection.Overlap;
} }
if (between(a, b, c) && between(c, d, b)) if (between(a, b, c) && between(c, d, b))
{ {
vCopy(ref p, c); p = c;
vCopy(ref q, b); q = b;
return Intersection.Overlap; return Intersection.Overlap;
} }
if (between(a, b, c) && between(c, d, a)) if (between(a, b, c) && between(c, d, a))
{ {
vCopy(ref p, c); p = c;
vCopy(ref q, a); q = a;
return Intersection.Overlap; return Intersection.Overlap;
} }
if (between(a, b, d) && between(c, d, b)) if (between(a, b, d) && between(c, d, b))
{ {
vCopy(ref p, d); p = d;
vCopy(ref q, b); q = b;
return Intersection.Overlap; return Intersection.Overlap;
} }
if (between(a, b, d) && between(c, d, a)) if (between(a, b, d) && between(c, d, a))
{ {
vCopy(ref p, d); p = d;
vCopy(ref q, a); q = a;
return Intersection.Overlap; return Intersection.Overlap;
} }

View File

@ -58,7 +58,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, startPos); startNode.pos = startPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = vDist(startPos, endPos) * H_SCALE; startNode.total = vDist(startPos, endPos) * H_SCALE;
@ -657,7 +657,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, centerPos); startNode.pos = centerPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = 0; startNode.total = 0;

View File

@ -325,7 +325,7 @@ namespace DotRecast.Detour
private static NavMeshParams getNavMeshParams(MeshData data) private static NavMeshParams getNavMeshParams(MeshData data)
{ {
NavMeshParams option = new NavMeshParams(); NavMeshParams option = new NavMeshParams();
vCopy(ref option.orig, data.header.bmin); option.orig = data.header.bmin;
option.tileWidth = data.header.bmax[0] - data.header.bmin[0]; option.tileWidth = data.header.bmax[0] - data.header.bmin[0];
option.tileHeight = data.header.bmax[2] - data.header.bmin[2]; option.tileHeight = data.header.bmax[2] - data.header.bmin[2];
option.maxTiles = 1; option.maxTiles = 1;
@ -1325,7 +1325,7 @@ namespace DotRecast.Detour
MeshTile tile = tileAndPoly.Item1; MeshTile tile = tileAndPoly.Item1;
Poly poly = tileAndPoly.Item2; Poly poly = tileAndPoly.Item2;
Vector3f closest = new Vector3f(); Vector3f closest = new Vector3f();
vCopy(ref closest, pos); closest = pos;
float? h = getPolyHeight(tile, poly, pos); float? h = getPolyHeight(tile, poly, pos);
if (null != h) if (null != h)
{ {

View File

@ -343,8 +343,8 @@ namespace DotRecast.Detour
hmax += option.walkableClimb; hmax += option.walkableClimb;
Vector3f bmin = new Vector3f(); Vector3f bmin = new Vector3f();
Vector3f bmax = new Vector3f(); Vector3f bmax = new Vector3f();
vCopy(ref bmin, option.bmin); bmin = option.bmin;
vCopy(ref bmax, option.bmax); bmax = option.bmax;
bmin[1] = hmin; bmin[1] = hmin;
bmax[1] = hmax; bmax[1] = hmax;
@ -467,8 +467,8 @@ namespace DotRecast.Detour
header.polyCount = totPolyCount; header.polyCount = totPolyCount;
header.vertCount = totVertCount; header.vertCount = totVertCount;
header.maxLinkCount = maxLinkCount; header.maxLinkCount = maxLinkCount;
vCopy(ref header.bmin, option.bmin); header.bmin = option.bmin;
vCopy(ref header.bmax, option.bmax); header.bmax = option.bmax;
header.detailMeshCount = option.polyCount; header.detailMeshCount = option.polyCount;
header.detailVertCount = uniqueDetailVertCount; header.detailVertCount = uniqueDetailVertCount;
header.detailTriCount = detailTriCount; header.detailTriCount = detailTriCount;

View File

@ -264,7 +264,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, centerPos); startNode.pos = centerPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = 0; startNode.total = 0;
@ -812,7 +812,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, startPos); startNode.pos = startPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = heuristic.getCost(startPos, endPos); startNode.total = heuristic.getCost(startPos, endPos);
@ -1061,8 +1061,8 @@ namespace DotRecast.Detour
m_query.status = Status.FAILURE; m_query.status = Status.FAILURE;
m_query.startRef = startRef; m_query.startRef = startRef;
m_query.endRef = endRef; m_query.endRef = endRef;
vCopy(ref m_query.startPos, startPos); m_query.startPos = startPos;
vCopy(ref m_query.endPos, endPos); m_query.endPos = endPos;
m_query.filter = filter; m_query.filter = filter;
m_query.options = options; m_query.options = options;
m_query.heuristic = heuristic; m_query.heuristic = heuristic;
@ -1094,7 +1094,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, startPos); startNode.pos = startPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = heuristic.getCost(startPos, endPos); startNode.total = heuristic.getCost(startPos, endPos);
@ -1842,7 +1842,7 @@ namespace DotRecast.Detour
Vector3f bestPos = new Vector3f(); Vector3f bestPos = new Vector3f();
float bestDist = float.MaxValue; float bestDist = float.MaxValue;
Node bestNode = null; Node bestNode = null;
vCopy(ref bestPos, startPos); bestPos = startPos;
// Search constraints // Search constraints
var searchPos = vLerp(startPos, endPos, 0.5f); var searchPos = vLerp(startPos, endPos, 0.5f);
@ -1874,7 +1874,7 @@ namespace DotRecast.Detour
if (pointInPolygon(endPos, verts, nverts)) if (pointInPolygon(endPos, verts, nverts))
{ {
bestNode = curNode; bestNode = curNode;
vCopy(ref bestPos, endPos); bestPos = endPos;
break; break;
} }
@ -2243,7 +2243,7 @@ namespace DotRecast.Detour
Vector3f curPos = new Vector3f(), lastPos = new Vector3f(); Vector3f curPos = new Vector3f(), lastPos = new Vector3f();
vCopy(ref curPos, startPos); curPos = startPos;
var dir = vSub(endPos, startPos); var dir = vSub(endPos, startPos);
MeshTile prevTile, tile, nextTile; MeshTile prevTile, tile, nextTile;
@ -2411,7 +2411,7 @@ namespace DotRecast.Detour
{ {
// compute the intersection point at the furthest end of the polygon // compute the intersection point at the furthest end of the polygon
// and correct the height (since the raycast moves in 2d) // and correct the height (since the raycast moves in 2d)
vCopy(ref lastPos, curPos); lastPos = curPos;
curPos = vMad(startPos, dir, hit.t); curPos = vMad(startPos, dir, hit.t);
VectorPtr e1 = new VectorPtr(verts, iresult.segMax * 3); VectorPtr e1 = new VectorPtr(verts, iresult.segMax * 3);
VectorPtr e2 = new VectorPtr(verts, ((iresult.segMax + 1) % nv) * 3); VectorPtr e2 = new VectorPtr(verts, ((iresult.segMax + 1) % nv) * 3);
@ -2516,7 +2516,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, centerPos); startNode.pos = centerPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = 0; startNode.total = 0;
@ -3243,7 +3243,7 @@ namespace DotRecast.Detour
m_openList.clear(); m_openList.clear();
Node startNode = m_nodePool.getNode(startRef); Node startNode = m_nodePool.getNode(startRef);
vCopy(ref startNode.pos, centerPos); startNode.pos = centerPos;
startNode.pidx = 0; startNode.pidx = 0;
startNode.cost = 0; startNode.cost = 0;
startNode.total = 0; startNode.total = 0;

View File

@ -435,7 +435,7 @@ public class CrowdTool : Tool
dd.begin(LINES, 3.0f); dd.begin(LINES, 3.0f);
Vector3f prev = new Vector3f(); Vector3f prev = new Vector3f();
float preva = 1; float preva = 1;
vCopy(ref prev, pos); prev = pos;
for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j) for (int j = 0; j < AGENT_MAX_TRAIL - 1; ++j)
{ {
int idx = (trail.htrail + AGENT_MAX_TRAIL - j) % AGENT_MAX_TRAIL; int idx = (trail.htrail + AGENT_MAX_TRAIL - j) % AGENT_MAX_TRAIL;

View File

@ -260,7 +260,7 @@ public class TestNavmeshTool : Tool
if (endOfPath && PathUtils.inRange(iterPos, steerTarget.steerPos, SLOP, 1.0f)) if (endOfPath && PathUtils.inRange(iterPos, steerTarget.steerPos, SLOP, 1.0f))
{ {
// Reached end of path. // Reached end of path.
vCopy(ref iterPos, targetPos); iterPos = targetPos;
if (m_smoothPath.Count < MAX_SMOOTH) if (m_smoothPath.Count < MAX_SMOOTH)
{ {
m_smoothPath.Add(iterPos); m_smoothPath.Add(iterPos);
@ -302,7 +302,7 @@ public class TestNavmeshTool : Tool
} }
// Move position at the other side of the off-mesh link. // Move position at the other side of the off-mesh link.
vCopy(ref iterPos, endPos); iterPos = endPos;
iterPos[1] = m_navQuery.getPolyHeight(polys[0], iterPos).result; iterPos[1] = m_navQuery.getPolyHeight(polys[0], iterPos).result;
} }
} }
@ -990,7 +990,7 @@ public class TestNavmeshTool : Tool
{ {
// In case of partial path, make sure the end point is clamped to the last polygon. // In case of partial path, make sure the end point is clamped to the last polygon.
Vector3f epos = new Vector3f(); Vector3f epos = new Vector3f();
vCopy(ref epos, m_epos); epos = m_epos;
if (m_polys[m_polys.Count - 1] != m_endRef) if (m_polys[m_polys.Count - 1] != m_endRef)
{ {
Result<ClosestPointOnPolyResult> result = m_navQuery Result<ClosestPointOnPolyResult> result = m_navQuery

View File

@ -58,7 +58,7 @@ public class MeshSetReaderWriterTest
NavMeshSetHeader header = new NavMeshSetHeader(); NavMeshSetHeader header = new NavMeshSetHeader();
header.magic = NavMeshSetHeader.NAVMESHSET_MAGIC; header.magic = NavMeshSetHeader.NAVMESHSET_MAGIC;
header.version = NavMeshSetHeader.NAVMESHSET_VERSION; header.version = NavMeshSetHeader.NAVMESHSET_VERSION;
vCopy(ref header.option.orig, geom.getMeshBoundsMin()); header.option.orig = geom.getMeshBoundsMin();
header.option.tileWidth = m_tileSize * m_cellSize; header.option.tileWidth = m_tileSize * m_cellSize;
header.option.tileHeight = m_tileSize * m_cellSize; header.option.tileHeight = m_tileSize * m_cellSize;
header.option.maxTiles = m_maxTiles; header.option.maxTiles = m_maxTiles;

View File

@ -54,7 +54,7 @@ public class AbstractTileCacheTest
int[] twh = Recast.Recast.calcTileCount(geom.getMeshBoundsMin(), geom.getMeshBoundsMax(), m_cellSize, m_tileSize, m_tileSize); int[] twh = Recast.Recast.calcTileCount(geom.getMeshBoundsMin(), geom.getMeshBoundsMax(), m_cellSize, m_tileSize, m_tileSize);
option.ch = m_cellHeight; option.ch = m_cellHeight;
option.cs = m_cellSize; option.cs = m_cellSize;
vCopy(ref option.orig, geom.getMeshBoundsMin()); option.orig = geom.getMeshBoundsMin();
option.height = m_tileSize; option.height = m_tileSize;
option.width = m_tileSize; option.width = m_tileSize;
option.walkableHeight = m_agentHeight; option.walkableHeight = m_agentHeight;

View File

@ -98,8 +98,8 @@ public class TestTileLayerBuilder : AbstractTileLayersBuilder
header.tx = tx; header.tx = tx;
header.ty = ty; header.ty = ty;
header.tlayer = i; header.tlayer = i;
vCopy(ref header.bmin, layer.bmin); header.bmin = layer.bmin;
vCopy(ref header.bmax, layer.bmax); header.bmax = layer.bmax;
// Tile info. // Tile info.
header.width = layer.width; header.width = layer.width;