public unsafe static void SetHitPointWorld(this ClosestConvexResultCallback obj, ref OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
obj.HitPointWorld = *(BulletSharp.Math.Vector3 *)valuePtr;
}
}
public unsafe static void GetHitNormalWorld(this ClosestConvexResultCallback obj, out OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
*(BulletSharp.Math.Vector3 *)valuePtr = obj.HitNormalWorld;
}
}
public void Cast(CollisionWorld cw)
{
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (var cb = new ClosestConvexResultCallback(ref source[i], ref dest[i]))
{
Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f);
Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f);
Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]);
Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]);
cw.ConvexSweepTestRef(boxShape, ref from, ref to, cb);
if (cb.HasHit)
{
hit_surface[i] = cb.HitPointWorld;
hit_com[i] = Vector3.Lerp(source[i], dest[i], cb.ClosestHitFraction);
hit_fraction[i] = cb.ClosestHitFraction;
normal[i] = cb.HitNormalWorld;
normal[i].Normalize();
}
else
{
hit_com[i] = dest[i];
hit_surface[i] = dest[i];
hit_fraction[i] = 1.0f;
normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
float mean_ms = (float)sum_ms / (float)sum_ms_samples;
Console.WriteLine("{0} rays in {1} ms {2} {3} {4}", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
}
protected override void OnInitializePhysics()
{
// collision configuration contains default setup for memory, collision setup
CollisionConf = new DefaultCollisionConfiguration();
Dispatcher = new CollisionDispatcher(CollisionConf);
Broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));
Solver = new SequentialImpulseConstraintSolver();
World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, Solver, CollisionConf);
World.DispatchInfo.AllowedCcdPenetration = 0.0001f;
//World.Gravity = Freelook.Up * -10.0f;
Matrix startTransform = Matrix.Translation(10.210098f, -1.6433364f, 16.453260f);
ghostObject = new PairCachingGhostObject();
ghostObject.WorldTransform = startTransform;
Broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback());
const float characterHeight = 1.75f;
const float characterWidth = 1.75f;
ConvexShape capsule = new CapsuleShape(characterWidth, characterHeight);
ghostObject.CollisionShape = capsule;
ghostObject.CollisionFlags = CollisionFlags.CharacterObject;
const float stepHeight = 0.35f;
character = new KinematicCharacterController(ghostObject, capsule, stepHeight);
BspLoader bspLoader = new BspLoader();
bspLoader.LoadBspFile("data/BspDemo.bsp");
BspConverter bsp2Bullet = new BspToBulletConverter(this);
bsp2Bullet.ConvertBsp(bspLoader, 0.1f);
World.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
World.AddAction(character);
convexResultCallback = new ClosestConvexResultCallback();
convexResultCallback.CollisionFilterMask = (short)CollisionFilterGroups.StaticFilter;
cameraSphere = new SphereShape(0.2f);
}
public void Cast(CollisionWorld cw, float frameDelta)
{
using (var cb = new ClosestConvexResultCallback())
{
for (int i = 0; i < NumRays; i++)
{
cb.ClosestHitFraction = 1.0f;
cb.ConvexFromWorld = _source[i];
cb.ConvexToWorld = _destination[i];
Matrix from = _fromRotation * Matrix.Translation(_source[i]);
Matrix to = _toRotation * Matrix.Translation(_destination[i]);
cw.ConvexSweepTestRef(_boxShape, ref from, ref to, cb);
if (cb.HasHit)
{
_hitPoint[i] = cb.HitPointWorld;
Vector3.Lerp(ref _source[i], ref _destination[i], cb.ClosestHitFraction, out _hitCenterOfMass[i]);
_hitFraction[i] = cb.ClosestHitFraction;
_normal[i] = cb.HitNormalWorld;
_normal[i].Normalize();
}
else
{
_hitCenterOfMass[i] = _destination[i];
_hitPoint[i] = _destination[i];
_hitFraction[i] = 1.0f;
_normal[i] = new Vector3(1.0f, 0.0f, 0.0f);
}
}
}
_time += frameDelta;
_frameCount++;
if (_frameCount > 50)
{
if (_time < _timeMin) _timeMin = _time;
if (_time > _timeMax) _timeMax = _time;
_timeTotal += _time;
_sampleCount++;
float timeMean = _timeTotal / _sampleCount;
Console.WriteLine("{0} rays in {1} s, min {2}, max {3}, mean {4}",
NumRays * _frameCount,
_time.ToString("0.000", CultureInfo.InvariantCulture),
_timeMin.ToString("0.000", CultureInfo.InvariantCulture),
_timeMax.ToString("0.000", CultureInfo.InvariantCulture),
timeMean.ToString("0.000", CultureInfo.InvariantCulture));
_time = 0;
_frameCount = 0;
}
}
public static void SetHitPointWorld(this ClosestConvexResultCallback obj, OpenTK.Vector3 value)
{
SetHitPointWorld(obj, ref value);
}
public static OpenTK.Vector3 GetHitPointWorld(this ClosestConvexResultCallback obj)
{
OpenTK.Vector3 value;
GetHitPointWorld(obj, out value);
return(value);
}
public override void OnHandleInput()
{
Matrix xform = ghostObject.WorldTransform;
Vector3 forwardDir = new Vector3(xform.M31, xform.M32, xform.M33);
//Console.Write("forwardDir={0},{1},{2}\n", forwardDir[0], forwardDir[1], forwardDir[2]);
Vector3 upDir = new Vector3(xform.M21, xform.M22, xform.M23);
forwardDir.Normalize();
upDir.Normalize();
Vector3 pos = xform.Origin;
Vector3 walkDirection = Vector3.Zero;
const float walkVelocity = 1.1f * 4.0f;
float walkSpeed = walkVelocity * FrameDelta * 10;// * 0.0001f;
float turnSpeed = FrameDelta * 3;
if (Input.KeysDown.Contains(Keys.Left))
{
Matrix orn = xform;
orn.Row4 = new Vector4(0, 0, 0, 1);
orn *= Matrix.RotationAxis(upDir, -turnSpeed);
orn.Row4 = new Vector4(pos.X, pos.Y, pos.Z, 1);
ghostObject.WorldTransform = orn;
}
if (Input.KeysDown.Contains(Keys.Right))
{
Matrix orn = xform;
orn.Row4 = new Vector4(0, 0, 0, 1);
orn *= Matrix.RotationAxis(upDir, turnSpeed);
orn.Row4 = new Vector4(pos.X, pos.Y, pos.Z, 1);
ghostObject.WorldTransform = orn;
}
if (Input.KeysDown.Contains(Keys.Up))
{
walkDirection += forwardDir;
}
if (Input.KeysDown.Contains(Keys.Down))
{
walkDirection -= forwardDir;
}
Vector3 cameraPos = pos - forwardDir * 12 + upDir * 5;
//use the convex sweep test to find a safe position for the camera (not blocked by static geometry)
SphereShape cameraSphere = new SphereShape(0.2f);
ClosestConvexResultCallback cb = new ClosestConvexResultCallback(ref pos, ref cameraPos);
cb.CollisionFilterMask = (short)CollisionFilterGroups.StaticFilter;
Matrix posMatrix = Matrix.Translation(pos);
Matrix cameraPosMatrix = Matrix.Translation(cameraPos);
World.ConvexSweepTestRef(cameraSphere, ref posMatrix, ref cameraPosMatrix, cb);
cameraSphere.Dispose();
if (cb.HasHit)
{
cameraPos = Vector3.Lerp(pos, cameraPos, cb.ClosestHitFraction);
}
cb.Dispose();
Freelook.SetEyeTarget(cameraPos, pos);
character.SetWalkDirection(walkDirection * walkSpeed);
base.OnHandleInput();
}
