public ClimbInfo CheckWallsClimbability()
{
var ret = new ClimbInfo();
ret.CanHang = false;
ret.WallHit = ClimbType.None;
ret.EdgeHit = false;
ret.EdgeObject = null;
ret.FloorLimit = HeightInfo.FloorHit ? HeightInfo.FloorPoint.Z : -9e10f;
ret.CeilingLimit = HeightInfo.CeilingHit ? HeightInfo.CeilingPoint.Z : 9e10f;
ret.Point = Climb.Point;
if (!HeightInfo.WallsClimb)
{
return ret;
}
ret.Up = Vector3.UnitZ;
var pos = Transform.Origin;
var from = pos + Transform.Basis.Column2 * Bf.BBMax.Z - Transform.Basis.Column1 * ClimbR;
var to = from;
var t = ForwardSize + Bf.BBMax.Y;
to += Transform.Basis.Column1 * t;
var ccb = ConvexCb;
ccb.ClosestHitFraction = 1.0f;
ccb.HitCollisionObject = null;
var tr1 = new Transform();
tr1.SetIdentity();
tr1.Origin = from;
var tr2 = new Transform();
tr2.SetIdentity();
tr2.Origin = to;
BtEngineDynamicsWorld.ConvexSweepTest(ClimbSensor, ((Matrix4) tr1).ToBullet(), ((Matrix4) tr2).ToBullet(), ccb);
if (!ccb.HasHit)
{
return ret;
}
ret.Point = ccb.HitPointWorld.ToOpenTK();
ret.N = ccb.HitNormalWorld.ToOpenTK();
var wn2 = new[] {ret.N.X, ret.N.Y};
t = (float) Math.Sqrt(wn2[0] * wn2[0] + wn2[1] * wn2[1]);
wn2[0] /= t;
wn2[1] /= t;
ret.Right.X = -wn2[1];
ret.Right.Y = wn2[0];
ret.Right.Z = 0.0f;
// now we have wall normale in XOY plane. Let us check all flags
if (HeightInfo.WallsClimbDir.HasFlagSig(SectorFlag.ClimbNorth) && wn2[1] < -0.7f
|| HeightInfo.WallsClimbDir.HasFlagSig(SectorFlag.ClimbEast) && wn2[0] < -0.7f
|| HeightInfo.WallsClimbDir.HasFlagSig(SectorFlag.ClimbSouth) && wn2[1] > 0.7f
|| HeightInfo.WallsClimbDir.HasFlagSig(SectorFlag.ClimbWest) && wn2[0] > 0.7f)
{
ret.WallHit = ClimbType.HandsOnly;
}
if (ret.WallHit != ClimbType.None)
{
t = 0.67f * Height;
from -= Transform.Basis.Column2 * t;
to = from;
t = ForwardSize + Bf.BBMax.Y;
to += Transform.Basis.Column1 * t;
ccb.ClosestHitFraction = 1.0f;
ccb.HitCollisionObject = null;
tr1.SetIdentity();
tr1.Origin = from;
tr2.SetIdentity();
tr2.Origin = to;
BtEngineDynamicsWorld.ConvexSweepTest(ClimbSensor, ((Matrix4) tr1).ToBullet(), ((Matrix4) tr2).ToBullet(), ccb);
if (ccb.HasHit)
{
ret.WallHit = ClimbType.FullBody;
}
}
return ret;
}
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;
}
