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 unsafe static void SetHitNormalWorld(this ClosestRayResultCallback obj, ref OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
obj.HitNormalWorld = *(BulletSharp.Math.Vector3 *)valuePtr;
}
}
public unsafe static void GetRayToWorld(this ClosestRayResultCallback obj, out OpenTK.Vector3 value)
{
fixed(OpenTK.Vector3 *valuePtr = &value)
{
*(BulletSharp.Math.Vector3 *)valuePtr = obj.RayToWorld;
}
}
public ResolveStepUp(DynamicCharacterController controller, CollisionWorld world, short staticRaycatGroup, short staticRaycastMask)
{
_controller = controller;
_world = world;
_closestRay = new ClosestRayResultCallback();
_closestRay.CollisionFilterGroup = staticRaycatGroup;
_closestRay.CollisionFilterMask = staticRaycastMask;
}
public object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
using (var rayCallback = new ClosestRayResultCallback(ref from, ref to))
{
_dynamicsWorld.RayTestRef(ref from, ref to, rayCallback);
if (rayCallback.HasHit)
{
var body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null && body.HasContactResponse)
{
result.HitPointInWorld = rayCallback.HitPointWorld;
Vector3 hitNormalInWorld = rayCallback.HitNormalWorld;
hitNormalInWorld.Normalize();
result.HitNormalInWorld = hitNormalInWorld;
result.DistFraction = rayCallback.ClosestHitFraction;
return(body);
}
}
}
return(null);
}
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.F1:
MessageBox.Show(
"Move using WASD + shift\n" +
"Left click - point camera\n" +
"Right click - pick up an object using a Point2PointConstraint\n" +
"Right click + shift - pick up an object using a fixed Generic6DofConstraint\n" +
"Space - shoot box\n" +
"Q - quit\n\n" +
Graphics.InfoText,
"Help");
// Key release won't be captured
Input.KeysDown.Remove(Keys.F1);
break;
case Keys.F3:
IsDebugDrawEnabled = !IsDebugDrawEnabled;
break;
case Keys.F8:
Input.ClearKeyCache();
GraphicsLibraryManager.ExitWithReload = true;
Graphics.Form.Close();
break;
case Keys.F11:
Graphics.IsFullScreen = !Graphics.IsFullScreen;
break;
case (Keys.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
using (var serializer = new DefaultSerializer(maxSerializeBufferSize))
{
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0,
dataBytes.Length);
using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create))
{
file.Write(dataBytes, 0, dataBytes.Length);
}
}
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;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
World.RayTestRef(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
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);
*/
}
}
}
else
{
MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
if (multiCol != null && multiCol.MultiBody != null)
{
MultiBody mb = multiCol.MultiBody;
prevCanSleep = mb.CanSleep;
mb.CanSleep = false;
Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);
MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
pickingMultiBodyPoint2Point = p2p;
}
}
oldPickingDist = (pickPos - rayFrom).Length;
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
MovePickedBody();
}
}
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.RayTestRef(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
}
public static OpenTK.Vector3 GetRayToWorld(this ClosestRayResultCallback obj)
{
OpenTK.Vector3 value;
GetRayToWorld(obj, out value);
return(value);
}
public static void SetRayToWorld(this ClosestRayResultCallback obj, OpenTK.Vector3 value)
{
SetRayToWorld(obj, ref value);
}
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 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 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.Control | Keys.F):
const int maxSerializeBufferSize = 1024 * 1024 * 5;
DefaultSerializer serializer = new DefaultSerializer(maxSerializeBufferSize);
World.Serialize(serializer);
byte[] dataBytes = new byte[serializer.CurrentBufferSize];
System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length);
System.IO.FileStream file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create);
file.Write(dataBytes, 0, dataBytes.Length);
file.Dispose();
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;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTestRef(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.SetFrames(tempFrameOffsetA, pickCon.FrameOffsetB);
}
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 Object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
// RayResultCallback& resultCallback;
using (var rayCallback = new ClosestRayResultCallback(ref from, ref to))
{
m_dynamicsWorld.RayTestRef(ref from, ref to, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null && body.HasContactResponse)
{
result.HitPointInWorld = rayCallback.HitPointWorld;
Vector3 hitNormalInWorld = rayCallback.HitNormalWorld;
hitNormalInWorld.Normalize();
result.HitNormalInWorld = hitNormalInWorld;
result.DistFraction = rayCallback.ClosestHitFraction;
return body;
}
}
}
return null;
}
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;
ClosestRayResultCallback rayCallback = new 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;
}
}
}
}
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();
GraphicsLibraryManager.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;
ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo);
_world.RayTest(ref rayFrom, ref rayTo, rayCallback);
if (rayCallback.HasHit)
{
Vector3 pickPos = rayCallback.HitPointWorld;
RigidBody body = rayCallback.CollisionObject as RigidBody;
if (body != null)
{
if (!(body.IsStaticObject || body.IsKinematicObject))
{
pickedBody = body;
pickedBody.ActivationState = ActivationState.DisableDeactivation;
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);
*/
}
}
}
else
{
MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider;
if (multiCol != null && multiCol.MultiBody != null)
{
MultiBody mb = multiCol.MultiBody;
prevCanSleep = mb.CanSleep;
mb.CanSleep = false;
Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos);
MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos);
p2p.MaxAppliedImpulse = 2;
(World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
pickingMultiBodyPoint2Point = p2p;
}
}
oldPickingDist = (pickPos - rayFrom).Length;
}
rayCallback.Dispose();
}
}
}
else if (Input.MouseReleased == MouseButtons.Right)
{
RemovePickingConstraint();
}
// Mouse movement
if (Input.MouseDown == MouseButtons.Right)
{
MovePickedBody();
}
}
public Mesh3d RayCastMesh3d()
{
var dir = GetDirection();
ClosestRayResultCallback rrc = new ClosestRayResultCallback(Transformation.GetPosition() + dir, Transformation.GetPosition() + dir * 10000.0f);
World.Root.PhysicalWorld.RayTest(Transformation.GetPosition() + dir, Transformation.GetPosition() + dir * 10000.0f, rrc);
if(rrc.HasHit)
{
return rrc.CollisionObject.UserObject as Mesh3d;
}
else
return null;
}
public Vector3 RayCastPosition()
{
var dir = GetDirection();
ClosestRayResultCallback rrc = new ClosestRayResultCallback(Transformation.GetPosition() + dir, Transformation.GetPosition() + dir * 10000.0f);
World.Root.PhysicalWorld.RayTest(Transformation.GetPosition() + dir, Transformation.GetPosition() + dir * 10000.0f, rrc);
if(rrc.HasHit)
{
return rrc.HitPointWorld;
}
else
return Vector3.Zero;
}
