public ClimbInfo CheckClimbability(Vector3 offset, HeightInfo nfc, float testHeight)
{
Vector3 from, to;
float d;
var pos = Transform.Origin;
var t1 = new Transform();
var t2 = new Transform();
byte upFounded;
var castRay = new float[6];
// init callbacks functions
nfc.Cb = RayCb;
nfc.Ccb = ConvexCb;
var tmp = pos + offset;
var ret = new ClimbInfo();
ret.HeightInfo = CheckNextStep(offset + new Vector3(0, 0, 128), nfc);
ret.CanHang = false;
ret.EdgeHit = false;
ret.EdgeObject = null;
ret.FloorLimit = HeightInfo.FloorHit ? HeightInfo.FloorPoint.Z : -9e10f;
ret.CeilingLimit = HeightInfo.CeilingHit ? HeightInfo.CeilingPoint.Z : 9e10f;
if (nfc.CeilingHit && nfc.CeilingPoint.Z < ret.CeilingLimit)
{
ret.CeilingLimit = nfc.CeilingPoint.Z;
}
ret.Point = Climb.Point;
// check max height
if (HeightInfo.CeilingHit && tmp.Z > HeightInfo.CeilingPoint.Z - ClimbR - 1.0f)
{
tmp.Z = HeightInfo.CeilingPoint.Z - ClimbR - 1.0f;
}
// let's calculate edge
from.X = pos.X - Transform.Basis.Column1.X * ClimbR * 2.0f;
from.Y = pos.Y - Transform.Basis.Column1.Y * ClimbR * 2.0f;
from.Z = pos.Z;
to = tmp;
t1.SetIdentity();
t2.SetIdentity();
upFounded = 0;
testHeight = Math.Max(testHeight, MaxStepUpHeight);
d = pos.Z + Bf.BBMax.Z - testHeight;
to.CopyToArray(castRay, 0);
to.CopyToArray(castRay, 3);
castRay[5] -= d;
var n0 = Vector3.Zero;
var n1 = Vector3.Zero;
var n0d = 0.0f;
var n1d = 0.0f;
do
{
t1.Origin = from;
t2.Origin = to;
nfc.Ccb.ClosestHitFraction = 1.0f;
nfc.Ccb.HitCollisionObject = null;
BtEngineDynamicsWorld.ConvexSweepTest(ClimbSensor, ((Matrix4) t1).ToBullet(), ((Matrix4) t2).ToBullet(), nfc.Ccb);
if (nfc.Ccb.HasHit)
{
if (nfc.Ccb.HitNormalWorld.Z >= 0.1f)
{
upFounded = 1;
n0 = nfc.Ccb.HitNormalWorld.ToOpenTK();
n0d = -n0.Dot(nfc.Ccb.HitPointWorld.ToOpenTK());
}
if (upFounded != 0 && nfc.Ccb.HitNormalWorld.Z < 0.001f)
{
n1 = nfc.Ccb.HitNormalWorld.ToOpenTK();
n1d = -n1.Dot(nfc.Ccb.HitPointWorld.ToOpenTK());
Climb.EdgeObject = nfc.Ccb.HitCollisionObject;
upFounded = 2;
break;
}
}
else
{
tmp.X = to.X;
tmp.Y = to.Y;
tmp.Z = d;
t1.Origin = to;
t2.Origin = tmp;
t1.Origin = from;
t2.Origin = to;
nfc.Ccb.ClosestHitFraction = 1.0f;
nfc.Ccb.HitCollisionObject = null;
BtEngineDynamicsWorld.ConvexSweepTest(ClimbSensor, ((Matrix4) t1).ToBullet(), ((Matrix4) t2).ToBullet(), nfc.Ccb);
if (nfc.Ccb.HasHit)
{
upFounded = 1;
n0 = nfc.Ccb.HitNormalWorld.ToOpenTK();
n0d = -n0.Dot(nfc.Ccb.HitPointWorld.ToOpenTK());
}
else
{
return ret;
}
}
// mult 0.66 is magic, but it must be less than 1.0 and greater than 0.0;
// close to 1.0 - bad precision, good speed;
// close to 0.0 - bad speed, bad precision;
// close to 0.5 - middle speed, good precision
from.Z -= 0.66f * ClimbR;
to.Z -= 0.66f * ClimbR;
} while (to.Z >= d); // we can't climb under floor!
if (upFounded != 2)
{
return ret;
}
// get the character plane equation
var n2 = Transform.Basis.Column0;
var n2d = -n2.Dot(pos);
Assert(!n0.FuzzyZero());
Assert(!n1.FuzzyZero());
Assert(!n2.FuzzyZero());
/*
* Solve system of the linear equations by Kramer method!
* I know - It may be slow, but it has a good precision!
* The root is point of 3 planes intersection.
*/
d = -n0[0] * (n1[1] * n2[2] - n1[2] * n2[1]) +
n1[0] * (n0[1] * n2[2] - n0[2] * n2[1]) -
n2[0] * (n0[1] * n1[2] - n0[2] * n1[1]);
if (Math.Abs(d) < 0.005f)
{
return ret;
}
ret.EdgePoint[0] = n0d * (n1[1] * n2[2] - n1[2] * n2[1]) -
n1d * (n0[1] * n2[2] - n0[2] * n2[1]) +
n2d * (n0[1] * n1[2] - n0[2] * n1[1]);
ret.EdgePoint[0] /= d;
ret.EdgePoint[1] = n0[0] * (n1d * n2[2] - n1[2] * n2d) -
n1[0] * (n0d * n2[2] - n0[2] * n2d) +
n2[0] * (n0d * n1[2] - n0[2] * n1d);
ret.EdgePoint[1] /= d;
ret.EdgePoint[2] = n0[0] * (n1[1] * n2d - n1d * n2[1]) -
n1[0] * (n0[1] * n2d - n0d * n2[1]) +
n2[0] * (n0[1] * n1d - n0d * n1[1]);
ret.EdgePoint[2] /= d;
ret.Point = ret.EdgePoint;
ret.Point.CopyToArray(castRay, 3);
/*
* unclimbable edge slant %)
*/
n2 = n0.Cross(n1);
d = CriticalSlantZComponent;
d *= d * (n2[0] * n2[0] + n2[1] * n2[1] + n2[2] * n2[2]);
if (n2[2] * n2[2] > d)
{
return ret;
}
/*
* Now, let us calculate z_angle
*/
ret.EdgeHit = true;
n2.Z = n2.X;
n2.X = n2.Y;
n2.Y = -n2.Z;
n2.Z = 0.0f;
if (n2.X * Transform.Basis.Column1.X + n2.Y * Transform.Basis.Column1.Y > 0) // direction fixing
{
n2.X = -n2.X;
n2.Y = -n2.Y;
}
ret.N = n2;
ret.Up.X = 0.0f;
ret.Up.Y = 0.0f;
ret.Up.Z = 1.0f;
ret.EdgeZAngle = Helper.Atan2(n2.X, -n2.Y) * DegPerRad;
ret.EdgeTanXY.X = -n2.Y;
ret.EdgeTanXY.Y = n2.X;
ret.EdgeTanXY.Z = 0.0f;
ret.EdgeTanXY /= (float) Math.Sqrt(n2.X * n2.X + n2.Y * n2.Y);
ret.Right = ret.EdgeTanXY;
if (!HeightInfo.FloorHit || ret.EdgePoint.Z - HeightInfo.FloorPoint.Z >= Height)
{
ret.CanHang = true;
}
ret.NextZSpace = 2.0f * Height;
if (nfc.FloorHit && nfc.CeilingHit)
{
ret.NextZSpace = nfc.CeilingPoint.Z - nfc.FloorPoint.Z;
}
return ret;
}
/// <summary>
/// Calculates next floor info + phantom filter + returns step info.
/// Current height info must be calculated!
/// </summary>
public StepType CheckNextStep(Vector3 offset, HeightInfo nfc)
{
float delta;
Vector3 from, to;
StepType ret;
var pos = Transform.Origin + offset;
GetHeightInfo(pos, nfc);
if (HeightInfo.FloorHit && nfc.FloorHit)
{
delta = nfc.FloorPoint.Z - HeightInfo.FloorPoint.Z;
if (Math.Abs(delta) < SPLIT_EPSILON)
{
from.Z = HeightInfo.FloorPoint.Z;
ret = StepType.Horizontal;
}
else if (delta < 0.0f)
{
delta = -delta;
from.Z = HeightInfo.FloorPoint.Z;
if (delta <= MinStepUpHeight)
{
ret = StepType.DownLittle;
}
else if (delta <= MaxStepUpHeight)
{
ret = StepType.DownBig;
}
else if (delta <= Height)
{
ret = StepType.DownDrop;
}
else
{
ret = StepType.DownCanHang;
}
}
else
{
from.Z = nfc.FloorPoint.Z;
if (delta <= MinStepUpHeight)
{
ret = StepType.UpLittle;
}
else if (delta <= MaxStepUpHeight)
{
ret = StepType.UpBig;
}
else if (delta <= Height)
{
ret = StepType.UpClimb;
}
else
{
ret = StepType.UpImpossible;
}
}
}
else if (!HeightInfo.FloorHit && !nfc.FloorHit)
{
from.Z = pos.Z;
ret = StepType.Horizontal;
}
else if (!HeightInfo.FloorHit && nfc.FloorHit)
{
from.Z = nfc.FloorPoint.Z;
ret = StepType.Horizontal;
}
else
{
from.Z = HeightInfo.FloorPoint.Z;
ret = StepType.DownCanHang;
}
from.Z += ClimbR;
to.Z = from.Z;
from.X = Transform.Origin.X;
from.Y = Transform.Origin.Y;
to.X = from.X;
to.Y = from.Y;
HeightInfo.Cb.ClosestHitFraction = 1.0f;
HeightInfo.Cb.CollisionObject = null;
BtEngineDynamicsWorld.RayTest(from.ToBullet(), to.ToBullet(), HeightInfo.Cb);
if (HeightInfo.Cb.HasHit)
{
ret = StepType.UpImpossible;
}
return ret;
}
public bool HasStopSlant(HeightInfo nextFc)
{
var pos = Transform.Origin;
var forward = Transform.Basis.Column1;
var floor = nextFc.FloorNormale;
return nextFc.FloorPoint.Z > pos.Z
&& nextFc.FloorNormale.Z < CriticalSlantZComponent
&& forward.X * floor.X + forward.Y * floor.Y < 0.0f;
}
/// <summary>
/// Start position are taken from transform
/// </summary>
public static void GetHeightInfo(Vector3 pos, HeightInfo fc, float vOffset = 0.0f)
{
var cb = fc.Cb;
var r = cb.Container?.Room;
RoomSector rs;
fc.FloorHit = false;
fc.CeilingHit = false;
fc.Water = false;
fc.Quicksand = QuicksandPosition.None;
fc.TransitionLevel = 32512.0f;
r = Room.FindPosCogerrence(pos, r)?.CheckFlip();
if (r != null)
{
rs = r.GetSectorXYZ(pos);
if (r.Flags.HasFlagUns(RoomFlag.Water))
{
while (rs.SectorAbove != null)
{
Assert(rs.SectorAbove != null);
rs = rs.SectorAbove.CheckFlip();
Assert(rs?.OwnerRoom != null);
if ((rs.OwnerRoom.Flags & (uint) RoomFlag.Water) == 0x00)
{
fc.TransitionLevel = rs.Floor;
fc.Water = true;
break;
}
}
}
else if (r.Flags.HasFlagUns(RoomFlag.Quicksand))
{
while (rs.SectorAbove != null)
{
Assert(rs.SectorAbove != null);
rs = rs.SectorAbove.CheckFlip();
Assert(rs?.OwnerRoom != null);
if ((rs.OwnerRoom.Flags & (uint) RoomFlag.Quicksand) == 0x00)
{
fc.TransitionLevel = rs.Floor;
fc.Quicksand = fc.TransitionLevel - fc.FloorPoint.Z > vOffset
? QuicksandPosition.Drowning
: QuicksandPosition.Sinking;
break;
}
}
}
else
{
while (rs.SectorBelow != null)
{
Assert(rs.SectorBelow != null);
rs = rs.SectorBelow.CheckFlip();
Assert(rs?.OwnerRoom != null);
if ((rs.OwnerRoom.Flags & (uint) RoomFlag.Water) != 0x00)
{
fc.TransitionLevel = rs.Ceiling;
fc.Water = true;
break;
}
else if ((rs.OwnerRoom.Flags & (uint) RoomFlag.Quicksand) != 0x00)
{
fc.TransitionLevel = rs.Ceiling;
fc.Quicksand = fc.TransitionLevel - fc.FloorPoint.Z > vOffset
? QuicksandPosition.Drowning
: QuicksandPosition.Sinking;
break;
}
}
}
}
// get heights
var from = pos;
var to = from;
to.Z -= 4096.0f;
cb.ClosestHitFraction = 1.0f;
cb.CollisionObject = null;
BtEngineDynamicsWorld.RayTest(from.ToBullet(), to.ToBullet(), cb);
fc.FloorHit = cb.HasHit;
if (fc.FloorHit)
{
fc.FloorNormale = cb.HitNormalWorld.ToOpenTK();
Helper.SetInterpolate3(out fc.FloorPoint, from, to, cb.ClosestHitFraction);
fc.FloorObject = cb.CollisionObject;
}
to = from;
to.Z += 4096.0f;
cb.ClosestHitFraction = 1.0f;
cb.CollisionObject = null;
BtEngineDynamicsWorld.RayTest(from.ToBullet(), to.ToBullet(), cb);
fc.CeilingHit = cb.HasHit;
if (fc.CeilingHit)
{
fc.CeilingNormale = cb.HitNormalWorld.ToOpenTK();
Helper.SetInterpolate3(out fc.CeilingPoint, from, to, cb.ClosestHitFraction);
fc.CeilingObject = cb.CollisionObject;
}
}