public bool IsStepBlocked(float x0, float y0, float z0,
float x1, float y1, float z1,
float toonHeight, float toonSize, TriangleCollection paintI)
{
TriangleCollection tc = GetChunkAt(x0, y0);
ICollection <int> ts, tsm, tst;
ts = null;
if (UseOctree)
{
TriangleOctree ot = tc.GetOctree();
tst = ts = ot.FindTrianglesInBox(Utils.min(x0, x1), Utils.min(y0, y1), Utils.min(z0, z1),
Utils.max(x0, x1), Utils.max(y0, y1), Utils.max(z0, z1));
}
if (UseMatrix)
{
TriangleMatrix tm = tc.GetTriangleMatrix();
tsm = ts = tm.GetAllInSquare(Utils.min(x0, x1), Utils.min(y0, y1), Utils.max(x0, x1), Utils.max(y0, y1));
}
bool coll = false;
Vector from, from_up;
Vector to, to_up;
from.x = x0; from.y = y0; from.z = z0 + toonSize;
from_up.x = x0; from_up.y = y0; from_up.z = z0 + toonHeight - toonSize;
to.x = x1; to.y = y1; to.z = z1 + toonSize;
to_up.x = x1; to_up.y = y1; to_up.z = z1 + toonHeight - toonSize;
foreach (int t in ts)
//for(int t = 0 ; t<tc.GetNumberOfTriangles(); t++)
{
Vector vertex0;
Vector vertex1;
Vector vertex2;
Vector intersect;
tc.GetTriangleVertices(t,
out vertex0.x, out vertex0.y, out vertex0.z,
out vertex1.x, out vertex1.y, out vertex1.z,
out vertex2.x, out vertex2.y, out vertex2.z);
if (Utils.SegmentTriangleIntersect(from, to_up, vertex0, vertex1, vertex2, out intersect) ||
Utils.SegmentTriangleIntersect(from_up, to, vertex0, vertex1, vertex2, out intersect))
{
if (paintI != null)
{
//paintI.AddMarker(intersect.x, intersect.y, intersect.z);
}
//Console.WriteLine("Collided at " + intersect);
coll = true;
return(true);
// blocked!
}
}
return(coll);
}
public bool IsStepBlocked(float x0, float y0, float z0,
float x1, float y1, float z1,
float toonHeight, float toonSize, TriangleCollection paintI)
{
TriangleCollection tc = GetChunkAt(x0, y0);
float dx = x0 - x1;
float dy = y0 - y1;
float dz = z0 - z1;
float stepLength = (float)Math.Sqrt(dx * dx + dy * dy + dz * dz);
// 1: check steepness
// TODO
// 2: check is there is a big step
float mid_x = (x0 + x1) / 2.0f;
float mid_y = (y0 + y1) / 2.0f;
float mid_z = (z0 + z1) / 2.0f;
float mid_z_hit = 0;
float mid_dz = Math.Abs(stepLength);
//if (mid_dz < 1.0f) mid_dz = 1.0f;
int mid_flags = 0;
if (FindStandableAt(mid_x, mid_y, mid_z - mid_dz, mid_z + mid_dz, out mid_z_hit, out mid_flags, toonHeight, toonSize))
{
float dz0 = Math.Abs(z0 - mid_z_hit);
float dz1 = Math.Abs(z1 - mid_z_hit);
// Console.WriteLine("z0 " + z0 + " z1 " + z1 + " dz0 " + dz0+ " dz1 " + dz1 );
if (dz0 > stepLength / 2.0 && dz0 > 1.0)
{
return(true); // too steep
}
if (dz1 > stepLength / 2.0 && dz1 > 1.0)
{
return(true); // too steep
}
}
else
{
// bad!
return(true);
}
ICollection <int> ts, tsm, tst;
ts = null;
if (UseOctree)
{
TriangleOctree ot = tc.GetOctree();
tst = ts = ot.FindTrianglesInBox(Utils.min(x0, x1), Utils.min(y0, y1), Utils.min(z0, z1),
Utils.max(x0, x1), Utils.max(y0, y1), Utils.max(z0, z1));
}
if (UseMatrix)
{
TriangleMatrix tm = tc.GetTriangleMatrix();
tsm = ts = tm.GetAllInSquare(Utils.min(x0, x1), Utils.min(y0, y1), Utils.max(x0, x1), Utils.max(y0, y1));
}
// 3: check collision with objects
Vector from, from_up, from_low;
Vector to, to_up, to_low;
from.x = x0;
from.y = y0;
from.z = z0 + toonSize; //+0.5
to.x = x1;
to.y = y1;
to.z = z1 + toonSize;
from_up = new Vector(from);
from_up.z = z0 + toonHeight - toonSize;
to_up = new Vector(to);
to_up.z = z1 + toonHeight - toonSize;
//diagonal
if (CheckForCollision(tc, ts, ref from, ref to_up))
{
return(true);
}
//diagonal
if (CheckForCollision(tc, ts, ref from_up, ref to))
{
return(true);
}
//head height
// if (CheckForCollision(tc, ts, ref from_up, ref to_up)) { return true; }
//close to the ground
from_low = new Vector(from);
from_low.z = z0 + 0.2f;
to_low = new Vector(to);
to_low.z = z1 + 0.2f;
if (CheckForCollision(tc, ts, ref from_low, ref to_low))
{
return(true);
}
float ddx, ddy;
GetNormal(x0, y0, x1, y1, out ddx, out ddy, 0.2f);
from_low.x += ddy;
from_low.y += ddx;
to_low.x += ddy;
to_low.y += ddx;
if (CheckForCollision(tc, ts, ref from_low, ref to_low))
{
return(true);
}
from_low.x -= 2 * ddy;
from_low.y -= 2 * ddx;
to_low.x -= 2 * ddy;
to_low.y -= 2 * ddx;
if (CheckForCollision(tc, ts, ref from_low, ref to_low))
{
return(true);
}
return(false);
}
public bool IsStepBlocked(float x0, float y0, float z0,
float x1, float y1, float z1,
float toonHeight, float toonSize, TriangleCollection paintI)
{
TriangleCollection tc = GetChunkAt(x0, y0);
float dx = x0 - x1;
float dy = y0 - y1;
float dz = z0 - z1;
var stepLength = (float)Math.Sqrt(dx * dx + dy * dy + dz + dz);
// 1: check steepness
// TODO
// 2: check is there is a big step
float mid_x = (x0 + x1) / 2.0f;
float mid_y = (y0 + y1) / 2.0f;
float mid_z = (z0 + z1) / 2.0f;
float mid_z_hit = 0;
float mid_dz = Math.Abs(stepLength);
//if (mid_dz < 1.0f) mid_dz = 1.0f;
int mid_flags = 0;
if (FindStandableAt(mid_x, mid_y, mid_z - mid_dz, mid_z + mid_dz, out mid_z_hit, out mid_flags, toonHeight,
toonSize))
{
float dz0 = Math.Abs(z0 - mid_z_hit);
float dz1 = Math.Abs(z1 - mid_z_hit);
// PathGraph.Log("z0 " + z0 + " z1 " + z1 + " dz0 " + dz0+ " dz1 " + dz1 );
if (dz0 > stepLength / 2.0 && dz0 > 1.0)
{
return(true); // too steep
}
if (dz1 > stepLength / 2.0 && dz1 > 1.0)
{
return(true); // too steep
}
}
else
{
// bad!
return(true);
}
ICollection <int> ts, tsm, tst;
ts = null;
if (UseOctree)
{
TriangleOctree ot = tc.GetOctree();
tst = ts = ot.FindTrianglesInBox(Utils.min(x0, x1), Utils.min(y0, y1), Utils.min(z0, z1),
Utils.max(x0, x1), Utils.max(y0, y1), Utils.max(z0, z1));
}
if (UseMatrix)
{
TriangleMatrix tm = tc.GetTriangleMatrix();
tsm = ts = tm.GetAllInSquare(Utils.min(x0, x1), Utils.min(y0, y1), Utils.max(x0, x1), Utils.max(y0, y1));
}
// 3: check collision with objects
bool coll = false;
Vector from, from_up;
Vector to, to_up;
from.x = x0;
from.y = y0;
from.z = z0 + toonSize;
from_up.x = x0;
from_up.y = y0;
from_up.z = z0 + toonHeight - toonSize;
to.x = x1;
to.y = y1;
to.z = z1 + toonSize;
to_up.x = x1;
to_up.y = y1;
to_up.z = z1 + toonHeight - toonSize;
foreach (int t in ts)
//for(int t = 0 ; t<tc.GetNumberOfTriangles(); t++)
{
Vector vertex0;
Vector vertex1;
Vector vertex2;
Vector intersect;
tc.GetTriangleVertices(t,
out vertex0.x, out vertex0.y, out vertex0.z,
out vertex1.x, out vertex1.y, out vertex1.z,
out vertex2.x, out vertex2.y, out vertex2.z);
if (Utils.SegmentTriangleIntersect(from, to_up, vertex0, vertex1, vertex2, out intersect) ||
Utils.SegmentTriangleIntersect(from_up, to, vertex0, vertex1, vertex2, out intersect))
{
if (paintI != null)
{
//paintI.AddMarker(intersect.x, intersect.y, intersect.z);
}
//PathGraph.Log("Collided at " + intersect);
coll = true;
return(true);
// blocked!
}
}
return(coll);
}