forked from mirror/DotRecast
105 lines
3.7 KiB
C#
105 lines
3.7 KiB
C#
using System;
|
|
using DotRecast.Core.Numerics;
|
|
|
|
namespace DotRecast.Detour.Extras.Jumplink
|
|
{
|
|
class EdgeSamplerFactory
|
|
{
|
|
public EdgeSampler Get(JumpLinkBuilderConfig acfg, JumpLinkType type, JumpEdge edge)
|
|
{
|
|
EdgeSampler es = null;
|
|
switch (type.Bit)
|
|
{
|
|
case JumpLinkType.EDGE_JUMP_BIT:
|
|
es = InitEdgeJumpSampler(acfg, edge);
|
|
break;
|
|
case JumpLinkType.EDGE_CLIMB_DOWN_BIT:
|
|
es = InitClimbDownSampler(acfg, edge);
|
|
break;
|
|
case JumpLinkType.EDGE_JUMP_OVER_BIT:
|
|
default:
|
|
throw new ArgumentException("Unsupported jump type " + type);
|
|
}
|
|
|
|
return es;
|
|
}
|
|
|
|
|
|
private EdgeSampler InitEdgeJumpSampler(JumpLinkBuilderConfig acfg, JumpEdge edge)
|
|
{
|
|
EdgeSampler es = new EdgeSampler(edge, new JumpTrajectory(acfg.jumpHeight));
|
|
es.start.height = acfg.agentClimb * 2;
|
|
RcVec3f offset = new RcVec3f();
|
|
Trans2d(ref offset, es.az, es.ay, new RcVec2f { X = acfg.startDistance, Y = -acfg.agentClimb, });
|
|
Vadd(ref es.start.p, edge.sp, offset);
|
|
Vadd(ref es.start.q, edge.sq, offset);
|
|
|
|
float dx = acfg.endDistance - 2 * acfg.agentRadius;
|
|
float cs = acfg.cellSize;
|
|
int nsamples = Math.Max(2, (int)MathF.Ceiling(dx / cs));
|
|
|
|
for (int j = 0; j < nsamples; ++j)
|
|
{
|
|
float v = (float)j / (float)(nsamples - 1);
|
|
float ox = 2 * acfg.agentRadius + dx * v;
|
|
Trans2d(ref offset, es.az, es.ay, new RcVec2f { X = ox, Y = acfg.minHeight });
|
|
GroundSegment end = new GroundSegment();
|
|
end.height = acfg.heightRange;
|
|
Vadd(ref end.p, edge.sp, offset);
|
|
Vadd(ref end.q, edge.sq, offset);
|
|
es.end.Add(end);
|
|
}
|
|
|
|
return es;
|
|
}
|
|
|
|
private EdgeSampler InitClimbDownSampler(JumpLinkBuilderConfig acfg, JumpEdge edge)
|
|
{
|
|
EdgeSampler es = new EdgeSampler(edge, new ClimbTrajectory());
|
|
es.start.height = acfg.agentClimb * 2;
|
|
RcVec3f offset = new RcVec3f();
|
|
Trans2d(ref offset, es.az, es.ay, new RcVec2f() { X = acfg.startDistance, Y = -acfg.agentClimb });
|
|
Vadd(ref es.start.p, edge.sp, offset);
|
|
Vadd(ref es.start.q, edge.sq, offset);
|
|
|
|
Trans2d(ref offset, es.az, es.ay, new RcVec2f() { X = acfg.endDistance, Y = acfg.minHeight });
|
|
GroundSegment end = new GroundSegment();
|
|
end.height = acfg.heightRange;
|
|
Vadd(ref end.p, edge.sp, offset);
|
|
Vadd(ref end.q, edge.sq, offset);
|
|
es.end.Add(end);
|
|
return es;
|
|
}
|
|
|
|
private void Vadd(float[] dest, float[] v1, float[] v2)
|
|
{
|
|
dest[0] = v1[0] + v2[0];
|
|
dest[1] = v1[1] + v2[1];
|
|
dest[2] = v1[2] + v2[2];
|
|
}
|
|
|
|
private void Vadd(ref RcVec3f dest, RcVec3f v1, RcVec3f v2)
|
|
{
|
|
dest.X = v1.X + v2.X;
|
|
dest.Y = v1.Y + v2.Y;
|
|
dest.Z = v1.Z + v2.Z;
|
|
}
|
|
|
|
|
|
private void Trans2d(float[] dst, float[] ax, float[] ay, float[] pt)
|
|
{
|
|
dst[0] = ax[0] * pt[0] + ay[0] * pt[1];
|
|
dst[1] = ax[1] * pt[0] + ay[1] * pt[1];
|
|
dst[2] = ax[2] * pt[0] + ay[2] * pt[1];
|
|
}
|
|
|
|
private void Trans2d(ref RcVec3f dst, RcVec3f ax, RcVec3f ay, RcVec2f pt)
|
|
{
|
|
dst.X = ax.X * pt.X + ay.X * pt.Y;
|
|
dst.Y = ax.Y * pt.X + ay.Y * pt.Y;
|
|
dst.Z = ax.Z * pt.X + ay.Z * pt.Y;
|
|
}
|
|
|
|
}
|
|
}
|