public void Cast(CollisionWorld cw) { for (int i = 0; i < NUMRAYS_IN_BAR; i++) { using (var cb = new CollisionWorld.ClosestConvexResultCallback(ref source[i], ref dest[i])) { Quaternion qFrom = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.0f); Quaternion qTo = Quaternion.RotationAxis(new Vector3(1.0f, 0.0f, 0.0f), 0.7f); Matrix from = Matrix.RotationQuaternion(qFrom) * Matrix.Translation(source[i]); Matrix to = Matrix.RotationQuaternion(qTo) * Matrix.Translation(dest[i]); cw.ConvexSweepTest(boxShape, from, to, cb); if (cb.HasHit) { hit_surface[i] = cb.HitPointWorld; hit_com[i] = Vector3.Lerp(source[i], dest[i], cb.ClosestHitFraction); hit_fraction[i] = cb.ClosestHitFraction; normal[i] = cb.HitNormalWorld; normal[i].Normalize(); } else { hit_com[i] = dest[i]; hit_surface[i] = dest[i]; hit_fraction[i] = 1.0f; normal[i] = new Vector3(1.0f, 0.0f, 0.0f); } } } frame_counter++; if (frame_counter > 50) { min_ms = ms < min_ms ? ms : min_ms; max_ms = ms > max_ms ? ms : max_ms; sum_ms += ms; sum_ms_samples++; float mean_ms = (float)sum_ms / (float)sum_ms_samples; Console.WriteLine("{0} rays in {1} ms {2} {3} {4}", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms); ms = 0; frame_counter = 0; } }
public void OnMouseMove(CollisionManager CollisionManager, Camera ActiveCamera, System.Windows.Forms.MouseEventArgs e) { var mouse = new { Near = ActiveCamera.Project(new Vector2(e.X, e.Y), depth: -0.90f), Far = ActiveCamera.Project(new Vector2(e.X, e.Y), depth: 1) }; Matrix4 from = Matrix4.Translation(mouse.Near); Matrix4 to = Matrix4.Translation(mouse.Far); var d = (mouse.Far - mouse.Near).Normalized(); using (var callback = new CollisionWorld.ClosestConvexResultCallback(mouse.Near, mouse.Far)) { callback.CollisionFilterGroup = CollisionFilterGroups.StaticFilter; callback.CollisionFilterMask = CollisionFilterGroups.StaticFilter; CollisionManager.World.ConvexSweepTest((ConvexShape)CollisionObject.CollisionShape, from, to, callback); var lookingNormal = (callback.ConvexToWorld - callback.ConvexFromWorld).Normalized(); var dot = Vector3.Dot(callback.HitNormalWorld, lookingNormal); Console.WriteLine(callback.HitNormalWorld); if (callback.HasHit) { if (!this.Selected) { return; } var matrix = this.WorldMatrix.ClearTranslation(); var collMtrix = this.Model.RenderModel.compressionInfo[0].ToExtentsMatrix(); Vector3 linVel, angVel; TransformUtil.CalculateVelocity(from, to, 1.0f, out linVel, out angVel); Matrix4 T; TransformUtil.IntegrateTransform(from, linVel, angVel, callback.ClosestHitFraction, out T); this.WorldMatrix = T; } } }
/// <summary> /// スィープ判定 /// </summary> /// <remarks> /// 指定のコリジョン形状を移動ベクトルだけ移動し、他のコリジョン物体と衝突するかどうかを検査します。 /// </remarks> /// <param name="obj">コリジョン オブジェクト</param> /// <param name="move">移動ベクトル</param> /// <returns></returns> public RaycastResult Sweep(CollisionObject obj, Vector3 move) { if (obj == null) { return new RaycastResult (); } var start = (obj.Node.Position + obj.Offset).ToBullet(); var end = start + move.ToBullet (); var proxy = obj.Data.BroadphaseHandle; var shape = obj.Data.CollisionShape; var rot = obj.Node.Orientation.ToBullet(); using (var result = new CollisionWorld.ClosestConvexResultCallback (start, end)) { result.CollisionFilterGroup = (CollisionFilterGroups)obj.Node.GroupID; result.CollisionFilterMask = (CollisionFilterGroups)obj.CollisionMask; var from = Matrix.RotationQuaternion(rot) * Matrix.Translation (start); var to = Matrix.RotationQuaternion(rot) * Matrix.Translation (end); // メモ: // ConvexSweepTest() は自分自身にもヒットする実装としない実装があるので、 // ここで一時的に自分のコリジョンを無効化する必要がある。 var tmp = proxy.CollisionFilterGroup; proxy.CollisionFilterGroup = CollisionFilterGroups.None; wld.ConvexSweepTest (shape as ConvexShape, from, to, result); proxy.CollisionFilterGroup = tmp; if (!result.HasHit) { return new RaycastResult (); } var frac = result.ClosestHitFraction; var dist = frac * move.Length; var hitNode = (result.CollisionObject.UserObject as CollisionObject).Node; var point = result.HitPointWorld.ToDD (); var normal = result.HitNormalWorld.ToDD (); return new RaycastResult (frac, dist, hitNode, point, normal); } }
public override void OnHandleInput() { Matrix xform = ghostObject.WorldTransform; Vector3 forwardDir = new Vector3(xform.M31, xform.M32, xform.M33); //Console.Write("forwardDir={0},{1},{2}\n", forwardDir[0], forwardDir[1], forwardDir[2]); Vector3 upDir = new Vector3(xform.M21, xform.M22, xform.M23); forwardDir.Normalize(); upDir.Normalize(); Vector3 pos = xform.Origin; Vector3 walkDirection = Vector3.Zero; const float walkVelocity = 1.1f * 4.0f; float walkSpeed = walkVelocity * FrameDelta * 10;// * 0.0001f; float turnSpeed = FrameDelta * 3; if (Input.KeysDown.Contains(Keys.Left)) { Matrix orn = xform; orn.set_Rows(3, new Vector4(0, 0, 0, 1)); orn *= Matrix.RotationAxis(upDir, -turnSpeed); orn.set_Rows(3, new Vector4(pos.X, pos.Y, pos.Z, 1)); ghostObject.WorldTransform = orn; } if (Input.KeysDown.Contains(Keys.Right)) { Matrix orn = xform; orn.set_Rows(3, new Vector4(0, 0, 0, 1)); orn *= Matrix.RotationAxis(upDir, turnSpeed); orn.set_Rows(3, new Vector4(pos.X, pos.Y, pos.Z, 1)); ghostObject.WorldTransform = orn; } if (Input.KeysDown.Contains(Keys.Up)) { walkDirection += forwardDir; } if (Input.KeysDown.Contains(Keys.Down)) { walkDirection -= forwardDir; } Vector3 cameraPos = pos - forwardDir * 12 + upDir * 5; //use the convex sweep test to find a safe position for the camera (not blocked by static geometry) SphereShape cameraSphere = new SphereShape(0.2f); CollisionWorld.ClosestConvexResultCallback cb = new CollisionWorld.ClosestConvexResultCallback(pos, cameraPos); cb.CollisionFilterMask = CollisionFilterGroups.StaticFilter; World.ConvexSweepTest(cameraSphere, Matrix.Translation(pos), Matrix.Translation(cameraPos), cb); cameraSphere.Dispose(); if (cb.HasHit) { cameraPos = Vector3.Lerp(pos, cameraPos, cb.ClosestHitFraction); } cb.Dispose(); Freelook.SetEyeTarget(cameraPos, pos); character.SetWalkDirection(walkDirection * walkSpeed); base.OnHandleInput(); }