private bool FindRealPoint(ref Matrix transform)
{
/* Look for the real height of the step.
* Move a bit along the ground-collision direction, horizontally,
* and vertically use 0 and the max step height. */
Vector3 minTargetPoint = new Vector3(_stepLocal.X, 0, _stepLocal.Z);
minTargetPoint *= 1 + _stepSearchOvershoot / minTargetPoint.Length;
// As above: we are using capsule lowest point, rather than ground
minTargetPoint.Y -= _originHeight;
Vector3 maxTargetPoint = minTargetPoint;
maxTargetPoint.Y += _controller.MaxStepHeight + _stepSearchOvershoot;
minTargetPoint = Vector3.TransformCoordinate(minTargetPoint, transform);
maxTargetPoint = Vector3.TransformCoordinate(maxTargetPoint, transform);
_closestRay.Setup(ref maxTargetPoint, ref minTargetPoint);
_world.RayTest(maxTargetPoint, minTargetPoint, _closestRay);
if (!_closestRay.HasHit)
{
return(false);
}
if ((1 - _closestRay.ClosestHitFraction) < 0.0001f)
{
// Almost at the minimum point, can walk normally
return(false);
}
RealPosWorld = _closestRay.HitPointWorld;
return(true);
}
public static CollisionObject ScreenPointToRay(Camera cam, Vector3 inputScreenCoords, CollisionFilterGroups rayFilterGroup, CollisionFilterGroups rayFilterMask)
{
/* Returns the first CollisionObject the ray hits.
* Requires as Input:
* - Camera cam, UnityCamera from which the ray should be cast, e.g. Camera.main
* - Vector3 inputScreenCoords, the screenpoint through which the ray should be cast. E.g. mousepointer Input.mousePosition
* - CollisionFilterGroups rayFilterGroup, of which FilterGroup the ray belongs to
* - CollisionFilterGroups rayFilterMask, which FilterMask the ray has (to map to other Objects FilterGroups
* Be aware the Bulletphysics probably needs Group/Mask to match in both ways, i.e. Ray needs to collide with otherObj, as otherObj needs to collide with Ray.
* To get all Ray hits, see commented out Callback AllHitsRayResultCallback below.
* First version. Adapt to your needs. Refer to Bulletphysics.org for info. Make Pull Request to Phong BulletUnity with improvements.*/
CollisionWorld collisionWorld = BPhysicsWorld.Get().world;
BulletSharp.Math.Vector3 rayFrom = cam.transform.position.ToBullet();
BulletSharp.Math.Vector3 rayTo = cam.ScreenToWorldPoint(new Vector3(inputScreenCoords.x, inputScreenCoords.y, cam.farClipPlane)).ToBullet();
BulletSharp.ClosestRayResultCallback rayCallBack = new BulletSharp.ClosestRayResultCallback(ref rayFrom, ref rayTo);
rayCallBack.CollisionFilterGroup = (short)rayFilterGroup;
rayCallBack.CollisionFilterMask = (short)rayFilterMask;
//BulletSharp.AllHitsRayResultCallback rayCallBack = new BulletSharp.AllHitsRayResultCallback(rayFrom, rayTo);
//Debug.Log("Casting ray from: " + rayFrom + " to: " + rayTo);
collisionWorld.RayTest(rayFrom, rayTo, rayCallBack);
closestRayResultReturnObj = null;
if (rayCallBack.HasHit)
{
//Debug.Log("rayCallBack " + rayCallBack.GetType() + " had a hit: " + rayCallBack.CollisionObject.UserObject + " / of type: " + rayCallBack.CollisionObject.UserObject.GetType());
closestRayResultReturnObj = rayCallBack.CollisionObject;
}
rayCallBack.Dispose();
return(closestRayResultReturnObj);
}
public void cast(CollisionWorld cw)
{
#if USE_BT_CLOCK
frame_timer.reset();
#endif //USE_BT_CLOCK
#if BATCH_RAYCASTER
if (gBatchRaycaster == null)
{
return;
}
gBatchRaycaster.clearRays();
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
gBatchRaycaster.addRay(source[i], dest[i]);
}
gBatchRaycaster.performBatchRaycast();
for (int i = 0; i < gBatchRaycaster.getNumRays(); i++)
{
const SpuRaycastTaskWorkUnitOut& outResult = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i], dest[i], outResult.hitFraction);
normal[i] = outResult.hitNormal;
normal[i] = normal[i].Normalize();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
ClosestRayResultCallback cb = new ClosestRayResultCallback(source[i], dest[i]);
cw.RayTest(ref source[i], ref dest[i], cb);
if (cb.HasHit())
{
hit[i] = cb.m_hitPointWorld;
normal[i] = cb.m_hitNormalWorld;
normal[i] = IndexedVector3.Normalize(normal[i]);
}
else
{
hit[i] = dest[i];
normal[i] = new IndexedVector3(1.0f, 0.0f, 0.0f);
}
}
#if USE_BT_CLOCK
ms += frame_timer.getTimeMilliseconds();
#endif //USE_BT_CLOCK
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;
//printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
#endif
}
public unsafe static void RayTest(this CollisionWorld obj, ref OpenTK.Vector3 rayFromWorld, ref OpenTK.Vector3 rayToWorld, RayResultCallback resultCallback)
{
fixed(OpenTK.Vector3 *rayFromWorldPtr = &rayFromWorld)
{
fixed(OpenTK.Vector3 *rayToWorldPtr = &rayToWorld)
{
obj.RayTest(ref *(BulletSharp.Math.Vector3 *)rayFromWorldPtr, ref *(BulletSharp.Math.Vector3 *)rayToWorldPtr, resultCallback);
}
}
}
public void Cast(CollisionWorld cw)
{
#if BATCH_RAYCASTER
if (!gBatchRaycaster)
{
return;
}
gBatchRaycaster->clearRays();
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
gBatchRaycaster->addRay(source[i], dest[i]);
}
gBatchRaycaster->performBatchRaycast();
for (int i = 0; i < gBatchRaycaster->getNumRays(); i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i], dest[i], out.HitFraction);
normal[i] = out.hitNormal;
normal[i].Normalize();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
using (var cb = new CollisionWorld.ClosestRayResultCallback(ref source[i], ref dest[i]))
{
cw.RayTest(ref source[i], ref dest[i], cb);
if (cb.HasHit)
{
hit[i] = cb.HitPointWorld;
normal[i] = cb.HitNormalWorld;
normal[i].Normalize();
}
else
{
hit[i] = dest[i];
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;
}
#endif
}
public RayResult FindEntityOnRay(Ray ray, PhysicsEntity ignore = null)
{
RayResult result = new RayResult();
if (collisionWorld != null)
{
BulletSharp.Math.Vector3 start = ray.Origin;
BulletSharp.Math.Vector3 end = ray.Origin + ray.Direction;
ClosestRayResultCallback callback = new ClosestRayResultCallback(ref start, ref end);
collisionWorld.RayTest(start, end, callback);
if (callback.HasHit && callback.CollisionObject != ignore.Rigidbody)
{
result.Position = callback.HitPointWorld;
result.Normal = callback.HitNormalWorld;
//result.Hit = callback.CollisionObject;
}
}
return(result);
}
public void Cast(CollisionWorld cw, float frameDelta)
{
#if BATCH_RAYCASTER
if (!batchRaycaster)
{
return;
}
batchRaycaster.ClearRays();
foreach (var ray in _rays)
{
batchRaycaster.AddRay(ray.Source, ray.Destination);
}
batchRaycaster.PerformBatchRaycast();
for (int i = 0; i < batchRaycaster.NumRays; i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*batchRaycaster)[i];
_rays[i].HitPoint.SetInterpolate3(_source[i], _destination[i], out.HitFraction);
_rays[i].Normal = out.hitNormal;
_rays[i].Normal.Normalize();
}
#else
foreach (var ray in _rays)
{
using (var cb = new ClosestRayResultCallback(ref ray.Source, ref ray.Destination))
{
cw.RayTest(ref ray.Source, ref ray.Destination, cb);
if (cb.HasHit)
{
ray.HitPoint = cb.HitPointWorld;
ray.Normal = cb.HitNormalWorld;
ray.Normal.Normalize();
}
else
{
ray.HitPoint = ray.Destination;
ray.Normal = 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;
PrintStats();
_time = 0;
_frameCount = 0;
}
#endif
}
new void Update()
{
var rayCalback = new KinematicClosestNotMeRayResultCallback(_ghostObject);
rayCalback.CollisionFilterMask = (int)CollisionFilleters.Look;
rayCalback.CollisionFilterGroup = (int)CollisionFilleters.Look;
//var test1 = Vector3.Cross(new Vector3(-6.84475f, 1.5930859f, 11.550375f) - camera.GetPos, (camera.GetLook * 10 * -Vector4.UnitZ).Xyz);
//Console.WriteLine("{0} {1} {2}", camera.GetPos.Convert(), (camera.GetLook * 10 * -Vector4.UnitZ).Xyz.Convert(), test1.Length / (camera.GetLook * 10 * -Vector4.UnitZ).Xyz.Length);
world.RayTest(camera.GetPos.Convert(), (camera.GetLook * 10 * new Vector4(0, 0, -1, 1)).Xyz.Convert(), rayCalback);
if (rayCalback.HasHit)
{
// Console.WriteLine("Hit ON {0}",rayCalback.HitNormalWorld);
if (rayCalback.CollisionObject.UserIndex == 1)
{
if (activeObject != rayCalback.CollisionObject)
{
activeObject = rayCalback.CollisionObject;
lookCallback?.Invoke(null);
lookCallback = (Action <SceneNode>)activeObject.UserObject;
}
lookCallback(camera.Node);
}
}
/*
* else if (activeObject != null)
* {
* //reset look
* lookCallback(Vector3.Zero);
* lookCallback = null;
* activeObject = null;
* }
*/
var keyState = GLWindow.gLWindow.KeyboardState;
if (game.IsFocused && keyState.IsKeyDown(Keys.Up))
{
Walk(-Vector3.UnitZ);
}
else if (game.IsFocused && keyState.IsKeyDown(Keys.Down))
{
Walk(Vector3.UnitZ);
}
else if (game.IsFocused && keyState.IsKeyDown(Keys.Left))
{
Walk(-Vector3.UnitX);
}
else if (game.IsFocused && keyState.IsKeyDown(Keys.Right))
{
Walk(Vector3.UnitX);
}
else if (game.IsFocused && keyState.IsKeyDown(Keys.Space))
{
Jump();
}
else
{
Stop();
}
base.Update();
}
public void Cast(CollisionWorld cw, float frameDelta)
{
#if BATCH_RAYCASTER
if (!gBatchRaycaster)
{
return;
}
gBatchRaycaster->clearRays();
for (int i = 0; i < NumRays; i++)
{
gBatchRaycaster->addRay(source[i], dest[i]);
}
gBatchRaycaster->performBatchRaycast();
for (int i = 0; i < gBatchRaycaster->getNumRays(); i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
_hitPoint[i].setInterpolate3(_source[i], _destination[i], out.HitFraction);
_normal[i] = out.hitNormal;
_normal[i].Normalize();
}
#else
for (int i = 0; i < NumRays; i++)
{
using (var cb = new ClosestRayResultCallback(ref _source[i], ref _destination[i]))
{
cw.RayTest(ref _source[i], ref _destination[i], cb);
if (cb.HasHit)
{
_hitPoint[i] = cb.HitPointWorld;
_normal[i] = cb.HitNormalWorld;
_normal[i].Normalize();
}
else
{
_hitPoint[i] = _destination[i];
_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;
}
#endif
}
