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; }