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