/// <summary>
/// Casts a ray in the physics world.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="distance">Distance the ray should travel</param>
/// <returns>RaycastResult or null if nothing hit.</returns>
public RaycastResult Raycast(Ray ray, float distance)
{
var from = ray.Origin.ToBullet();
var to = (ray.Origin + distance * ray.Direction).ToBullet();
var callback = new CollisionWorld.ClosestRayResultCallback(from, to);
this.world.RayTest(from, to, callback);
if (!callback.HasHit)
{
return(null);
}
var result = new RaycastResult();
result.Position = callback.HitPointWorld.ToOpenTK();
result.Normal = callback.HitNormalWorld.ToOpenTK();
result.RigidBody = callback.CollisionObject.UserObject as RigidBody;
if (result.RigidBody != null)
{
result.Node = result.RigidBody.Node;
}
return(result);
}
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 void Evaluate(int SpreadMax)
{
if (this.FWorld.PluginIO.IsConnected)
{
this.FHit.SliceCount = SpreadMax;
List<double> fraction = new List<double>();
List<Vector3D> position = new List<Vector3D>();
List<Vector3D> normal = new List<Vector3D>();
List<RigidBody> body = new List<RigidBody>();
List<int> bodyid = new List<int>();
List<int> qidx = new List<int>();
for (int i = 0; i < SpreadMax; i++)
{
Vector3 from = this.FFrom[i].ToBulletVector();
Vector3 to = this.FTo[i].ToBulletVector();
CollisionWorld.ClosestRayResultCallback cb =
new CollisionWorld.ClosestRayResultCallback(from,to );
//cb.CollisionFilterMask =
this.FWorld[0].World.RayTest(from, to, cb);
if (cb.HasHit)
{
this.FHit[i] = true;
BodyCustomData bd = (BodyCustomData)cb.CollisionObject.UserObject;
fraction.Add(cb.ClosestHitFraction);
position.Add(cb.HitPointWorld.ToVVVVector());
normal.Add(cb.HitNormalWorld.ToVVVVector());
body.Add((RigidBody)cb.CollisionObject);
bodyid.Add(bd.Id);
qidx.Add(i);
}
else
{
this.FHit[i] = false;
}
}
this.FId.AssignFrom(bodyid);
this.FHitFraction.AssignFrom(fraction);
this.FHitNormal.AssignFrom(normal);
this.FHitPosition.AssignFrom(position);
this.FQueryIndex.AssignFrom(qidx);
this.FBody.AssignFrom(body);
}
else
{
this.FHit.SliceCount = 0;
this.FId.SliceCount = 0;
this.FHitFraction.SliceCount = 0;
this.FHitPosition.SliceCount = 0;
}
}
public void Evaluate(int SpreadMax)
{
if (this.FWorld.PluginIO.IsConnected)
{
this.FHit.SliceCount = SpreadMax;
List <double> fraction = new List <double>();
List <Vector3D> position = new List <Vector3D>();
List <Vector3D> normal = new List <Vector3D>();
List <RigidBody> body = new List <RigidBody>();
List <int> bodyid = new List <int>();
List <int> qidx = new List <int>();
for (int i = 0; i < SpreadMax; i++)
{
Vector3 from = this.FFrom[i].ToBulletVector();
Vector3 to = this.FTo[i].ToBulletVector();
CollisionWorld.ClosestRayResultCallback cb =
new CollisionWorld.ClosestRayResultCallback(from, to);
//cb.CollisionFilterMask =
this.FWorld[0].World.RayTest(from, to, cb);
if (cb.HasHit)
{
this.FHit[i] = true;
BodyCustomData bd = (BodyCustomData)cb.CollisionObject.UserObject;
fraction.Add(cb.ClosestHitFraction);
position.Add(cb.HitPointWorld.ToVVVVector());
normal.Add(cb.HitNormalWorld.ToVVVVector());
body.Add((RigidBody)cb.CollisionObject);
bodyid.Add(bd.Id);
qidx.Add(i);
}
else
{
this.FHit[i] = false;
}
}
this.FId.AssignFrom(bodyid);
this.FHitFraction.AssignFrom(fraction);
this.FHitNormal.AssignFrom(normal);
this.FHitPosition.AssignFrom(position);
this.FQueryIndex.AssignFrom(qidx);
this.FBody.AssignFrom(body);
}
else
{
this.FHit.SliceCount = 0;
this.FId.SliceCount = 0;
this.FHitFraction.SliceCount = 0;
this.FHitPosition.SliceCount = 0;
}
}
private static CollisionWorld.ClosestRayResultCallback SetupCallback(
CollisionManager collision, Camera viewportCamera, System.Windows.Forms.MouseEventArgs e)
{
var mouse = new
{
Near = viewportCamera.Project(new Vector2(e.X, e.Y), depth: -1),
Far = viewportCamera.Project(new Vector2(e.X, e.Y), depth: 1)
};
var callback = new CollisionWorld.ClosestRayResultCallback(
mouse.Near,
mouse.Far);
callback.Flags = (uint)BulletSharp.InternalEdgeAdjustFlags.TriangleConvexBackfaceMode;
collision.World.RayTest(mouse.Near, mouse.Far, callback);
return(callback);
}
public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);
_dynamicsWorld.RayTest(from, to, rayCallback);
result = new VehicleRaycasterResult();
if (!rayCallback.HasHit) return 0;
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body == null) return 0;
result.HitPointInWorld = rayCallback.HitPointWorld;
result.HitNormalInWorld = rayCallback.HitNormalWorld;
result.HitNormalInWorld.Normalize();
result.DistFraction = rayCallback.ClosestHitFraction;
return body;
}
private static CollisionWorld.ClosestRayResultCallback SetupAllRayCallback(
CollisionManager collision, Camera viewportCamera, System.Windows.Forms.MouseEventArgs e)
{
var mouse = new
{
Near = viewportCamera.Project(new Vector2(e.X, e.Y), depth: -1),
Far = viewportCamera.Project(new Vector2(e.X, e.Y), depth: 1)
};
var callback = new CollisionWorld.ClosestRayResultCallback(
mouse.Near,
mouse.Far);
callback.CollisionFilterGroup = CollisionFilterGroups.StaticFilter;
callback.CollisionFilterMask = CollisionFilterGroups.StaticFilter;
collision.World.RayTest(mouse.Near, mouse.Far, callback);
return(callback);
}
public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);
_dynamicsWorld.RayTest(from, to, rayCallback);
result = new VehicleRaycasterResult();
if (!rayCallback.HasHit)
{
return(0);
}
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body == null)
{
return(0);
}
result.HitPointInWorld = rayCallback.HitPointWorld;
result.HitNormalInWorld = rayCallback.HitNormalWorld;
result.HitNormalInWorld.Normalize();
result.DistFraction = rayCallback.ClosestHitFraction;
return(body);
}
public virtual void OnHandleInput()
{
if (_input.KeysPressed.Count != 0)
{
switch (_input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
LibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(_freelook.Eye, GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (_input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView);
if (_input.MousePressed == MouseButtons.Right)
{
if (_world != null)
{
Vector3 rayFrom = _freelook.Eye;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (_input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
_world.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
rayCallback.Dispose();
}
}
}
else if (_input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (_input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
public virtual void OnHandleInput()
{
if (_input.KeysPressed.Count != 0)
{
switch (_input.KeysPressed[0])
{
case Keys.Escape:
case Keys.Q:
Graphics.Form.Close();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
LibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case Keys.G:
//shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
ShootBox(_freelook.Eye, GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView));
break;
case Keys.Return:
ClientResetScene();
break;
}
}
if (_input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView);
if (_input.MousePressed == MouseButtons.Right)
{
if (_world != null)
{
Vector3 rayFrom = _freelook.Eye;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (_input.KeysDown.Contains(Keys.ShiftKey))
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false)
{
LinearLowerLimit = Vector3.Zero,
LinearUpperLimit = Vector3.Zero,
AngularLowerLimit = Vector3.Zero,
AngularUpperLimit = Vector3.Zero
};
_world.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
rayCallback.Dispose();
}
}
}
else if (_input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (_input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(_input.MousePoint, _freelook.Eye, _freelook.Target, Graphics.FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = _freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
protected virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
Keys key = Input.KeysPressed[0];
switch (key)
{
case Keys.Escape:
case Keys.Q:
Quit();
return;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F11:
ToggleFullScreen();
break;
case Keys.Space:
PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView));
break;
}
}
if (Input.MousePressed != MouseButtonFlags.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (Input.MousePressed == MouseButtonFlags.RightDown)
{
if (PhysicsContext.World != null)
{
Vector3 rayFrom = Freelook.Eye;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(rayFrom, rayTo);
PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (use6Dof)
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false);
dof6.LinearLowerLimit = Vector3.Zero;
dof6.LinearUpperLimit = Vector3.Zero;
dof6.AngularLowerLimit = Vector3.Zero;
dof6.AngularUpperLimit = Vector3.Zero;
PhysicsContext.World.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
PhysicsContext.World.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
use6Dof = !use6Dof;
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
}
}
else if (Input.MousePressed == MouseButtonFlags.RightUp)
{
if (pickConstraint != null && PhysicsContext.World != null)
{
PhysicsContext.World.RemoveConstraint(pickConstraint);
pickConstraint.Dispose();
pickConstraint = null;
pickedBody.ForceActivationState(ActivationState.ActiveTag);
pickedBody.DeactivationTime = 0;
pickedBody = null;
}
}
}
// Mouse movement
if (Input.MouseDown == MouseButtonFlags.RightDown)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
/// <summary>
/// コリジョン形状1つに対するレイキャスト
/// </summary>
/// <param name="colA">キャスト対象のコリジョン オブジェクト</param>
/// <param name="start">開始地点（ワールド座標）</param>
/// <param name="end">終了地点（ワールド座標）</param>
/// <returns>ヒットした時はその地点までの距離(0以上)の値、ヒットしなかった時は <see cref="Single.NaN"/>.</returns>
public static float RayCast(CollisionObject colA, Vector3 start, Vector3 end)
{
var from = new BulletSharp.Vector3 (start.X, start.Y, start.Z);
var to = new BulletSharp.Vector3 (end.X, end.Y, end.Z);
var result = new CollisionWorld.ClosestRayResultCallback (from, to);
colA.ghostObject.RayTest (from, to, result);
if (!result.HasHit) {
return Single.NaN;
}
return result.ClosestHitFraction * (end - start).Length;
}
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
}
protected virtual void OnHandleInput()
{
if (Input.KeysPressed.Count != 0)
{
Keys key = Input.KeysPressed[0];
switch (key)
{
case Keys.Escape:
case Keys.Q:
Form.Close();
return;
case Keys.F3:
//IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F11:
//ToggleFullScreen();
break;
case Keys.G:
shadowsEnabled = !shadowsEnabled;
break;
case Keys.Space:
PhysicsContext.ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView));
break;
}
}
if (Input.MousePressed != MouseButtons.None)
{
Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (Input.MousePressed == MouseButtons.Right)
{
if (PhysicsContext.World != null)
{
Vector3 rayFrom = Freelook.Eye;
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(rayFrom, rayTo);
PhysicsContext.World.RayTest(rayFrom, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform));
if (use6Dof)
{
Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false);
dof6.LinearLowerLimit = Vector3.Zero;
dof6.LinearUpperLimit = Vector3.Zero;
dof6.AngularLowerLimit = Vector3.Zero;
dof6.AngularUpperLimit = Vector3.Zero;
PhysicsContext.World.AddConstraint(dof6);
pickConstraint = dof6;
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4);
dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4);
dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5);
}
else
{
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
PhysicsContext.World.AddConstraint(p2p);
pickConstraint = p2p;
p2p.Setting.ImpulseClamp = 30;
//very weak constraint for picking
p2p.Setting.Tau = 0.001f;
/*
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1);
* p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 0);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 1);
* p2p.SetParam(ConstraintParams.Erp, 0.1f, 2);
*/
}
use6Dof = !use6Dof;
oldPickingDist = (pickPos - rayFrom).Length();
}
}
}
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
if (pickConstraint != null)
{
Vector3 newRayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, FieldOfView);
if (pickConstraint.ConstraintType == TypedConstraintType.D6)
{
Generic6DofConstraint pickCon = pickConstraint as Generic6DofConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
Vector3 newPivotB = rayFrom + dir;
Matrix tempFrameOffsetA = pickCon.FrameOffsetA;
tempFrameOffsetA.M41 = newPivotB.X;
tempFrameOffsetA.M42 = newPivotB.Y;
tempFrameOffsetA.M43 = newPivotB.Z;
pickCon.FrameOffsetA = tempFrameOffsetA;
}
else
{
Point2PointConstraint pickCon = pickConstraint as Point2PointConstraint;
//keep it at the same picking distance
Vector3 rayFrom = Freelook.Eye;
Vector3 dir = newRayTo - rayFrom;
dir.Normalize();
dir *= oldPickingDist;
pickCon.PivotInB = rayFrom + dir;
}
}
}
}
/// <summary>
/// Called when the game has determined that game logic needs to be processed.
/// </summary>
/// <param name="gameTime">Time passed since the last call to this function.</param>
protected override void Update(GameTime gameTime)
{
MouseState mouseState = Mouse.GetState();
Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y);
if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released)
{
shootBox(rayTo);
}
if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released)
{
if (_world != null)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo);
_world.RayTest(_camera.Position, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null)
{
//other exclusions?
if (!(body.IsStaticObject || body.IsKinematicObject))
{
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform));
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
_pickConstraint = p2p;
//save mouse position for dragging
_oldPickingPos = rayTo;
Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z);
_oldPickingDist = (eyePos - pickPos).Length();
//very weak constraint for picking
p2p.Settings.Tau = 1.1f;
}
}
}
}
}
else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed)
{
if (_pickConstraint != null && _world != null)
{
_world.RemoveConstraint(_pickConstraint);
_pickConstraint = null;
_pickedBody.ForceActivationState(ActivationState.Active);
_pickedBody.DeactivationTime = 0f;
_pickedBody = null;
}
}
if (_pickConstraint != null)
{
//move the constraint pivot
Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint;
if (p2p != null)
{
//keep it at the same picking distance
Vector3 dir = rayTo - _camera.Position;
dir.Normalize();
dir *= _oldPickingDist;
Vector3 newPos = _camera.Position + dir;
p2p.PivotInB = newPos;
}
}
_prevMouseState = mouseState;
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
//world.stepSimulation(1.0f/60.0f,0);
int numObjects = _world.CollisionObjectsCount;
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = _world.CollisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.MotionState != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState;
myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform;
colObj.WorldTransform = myMotionState.GraphicsWorldTransform;
colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform;
colObj.Activate();
}
//removed cached contact points
_world.Broadphase.CleanProxyFromPairs(colObj.Broadphase);
if (body != null && !body.IsStaticObject)
{
RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0);
RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0);
}
}
}
}
else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
Exit();
}
else
{
//world.stepSimulation(1.0f / 60.0f, 1);
}
base.Update(gameTime);
}
