1 | #region MIT License |
---|
2 | /* |
---|
3 | * Copyright (c) 2005-2008 Jonathan Mark Porter. http://physics2d.googlepages.com/ |
---|
4 | * |
---|
5 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
---|
6 | * of this software and associated documentation files (the "Software"), to deal |
---|
7 | * in the Software without restriction, including without limitation the rights to |
---|
8 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of |
---|
9 | * the Software, and to permit persons to whom the Software is furnished to do so, |
---|
10 | * subject to the following conditions: |
---|
11 | * |
---|
12 | * The above copyright notice and this permission notice shall be |
---|
13 | * included in all copies or substantial portions of the Software. |
---|
14 | * |
---|
15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, |
---|
16 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR |
---|
17 | * PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE |
---|
18 | * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, |
---|
19 | * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
---|
20 | * OTHER DEALINGS IN THE SOFTWARE. |
---|
21 | */ |
---|
22 | #endregion |
---|
23 | |
---|
24 | |
---|
25 | |
---|
26 | |
---|
27 | #if UseDouble |
---|
28 | using Scalar = System.Double; |
---|
29 | #else |
---|
30 | using Scalar = System.Single; |
---|
31 | #endif |
---|
32 | using System; |
---|
33 | using System.Collections.Generic; |
---|
34 | using AdvanceMath; |
---|
35 | using AdvanceMath.Geometry2D; |
---|
36 | |
---|
37 | |
---|
38 | namespace Physics2DDotNet |
---|
39 | { |
---|
40 | |
---|
41 | |
---|
42 | /// <summary> |
---|
43 | /// contains some methods to do physics calculations. |
---|
44 | /// </summary> |
---|
45 | public static class PhysicsHelper |
---|
46 | { |
---|
47 | public const Scalar GravitationalConstant = 6.67e-11f; |
---|
48 | |
---|
49 | |
---|
50 | public static Vector2D GetTangent(Vector2D normal) |
---|
51 | { |
---|
52 | Vector2D result; |
---|
53 | GetTangent(ref normal, out result); |
---|
54 | return result; |
---|
55 | } |
---|
56 | public static void GetTangent(ref Vector2D normal, out Vector2D result) |
---|
57 | { |
---|
58 | Scalar normalX = normal.X; |
---|
59 | result.X = normal.Y; |
---|
60 | result.Y = -normalX; |
---|
61 | } |
---|
62 | |
---|
63 | |
---|
64 | public static Scalar GetMassNormal( |
---|
65 | Vector2D point1, Vector2D point2, Vector2D normal, |
---|
66 | Scalar mass1Inv, Scalar inertia1Inv, |
---|
67 | Scalar mass2Inv, Scalar inertia2Inv) |
---|
68 | { |
---|
69 | Scalar result; |
---|
70 | GetMassNormal( |
---|
71 | ref point1, ref point2, ref normal, |
---|
72 | ref mass1Inv, ref inertia1Inv, |
---|
73 | ref mass2Inv, ref inertia2Inv, |
---|
74 | out result); |
---|
75 | return result; |
---|
76 | } |
---|
77 | public static void GetMassNormal( |
---|
78 | ref Vector2D point1, ref Vector2D point2, ref Vector2D normal, |
---|
79 | ref Scalar mass1Inv, ref Scalar inertia1Inv, |
---|
80 | ref Scalar mass2Inv, ref Scalar inertia2Inv, |
---|
81 | out Scalar result) |
---|
82 | { |
---|
83 | Scalar rn1, rn2; |
---|
84 | Vector2D.Dot(ref point1, ref normal, out rn1); |
---|
85 | Vector2D.Dot(ref point2, ref normal, out rn2); |
---|
86 | result = |
---|
87 | 1.0f / |
---|
88 | (mass1Inv + mass2Inv + |
---|
89 | inertia1Inv * ((point1.X * point1.X + point1.Y * point1.Y) - rn1 * rn1) + |
---|
90 | inertia2Inv * ((point2.X * point2.X + point2.Y * point2.Y) - rn2 * rn2)); |
---|
91 | } |
---|
92 | |
---|
93 | public static Vector2D GetRelativeVelocity( |
---|
94 | ALVector2D velocity1, ALVector2D velocity2, |
---|
95 | Vector2D point1, Vector2D point2) |
---|
96 | { |
---|
97 | Vector2D result; |
---|
98 | GetRelativeVelocity(ref velocity1, ref velocity2, ref point1, ref point2, out result); |
---|
99 | return result; |
---|
100 | } |
---|
101 | |
---|
102 | public static void GetRelativeVelocity( |
---|
103 | ref ALVector2D velocity1, ref ALVector2D velocity2, |
---|
104 | ref Vector2D point1, ref Vector2D point2, |
---|
105 | out Vector2D result) |
---|
106 | { |
---|
107 | result.X = |
---|
108 | (velocity2.Linear.X - velocity2.Angular * point2.Y) - |
---|
109 | (velocity1.Linear.X - velocity1.Angular * point1.Y); |
---|
110 | result.Y = |
---|
111 | (velocity2.Linear.Y + velocity2.Angular * point2.X) - |
---|
112 | (velocity1.Linear.Y + velocity1.Angular * point1.X); |
---|
113 | } |
---|
114 | public static void GetRelativeVelocity( |
---|
115 | ref ALVector2D velocity1, ref Vector2D point1, out Vector2D result) |
---|
116 | { |
---|
117 | result.X = -(velocity1.Linear.X - velocity1.Angular * point1.Y); |
---|
118 | result.Y = -(velocity1.Linear.Y + velocity1.Angular * point1.X); |
---|
119 | } |
---|
120 | |
---|
121 | public static void AddImpulse( |
---|
122 | ref ALVector2D velocity, ref Vector2D impulse, ref Vector2D point, |
---|
123 | ref Scalar massInv, ref Scalar inertiaInv) |
---|
124 | { |
---|
125 | velocity.Linear.X += impulse.X * massInv; |
---|
126 | velocity.Linear.Y += impulse.Y * massInv; |
---|
127 | velocity.Angular += (inertiaInv * (point.X * impulse.Y - point.Y * impulse.X)); |
---|
128 | } |
---|
129 | |
---|
130 | public static void SubtractImpulse( |
---|
131 | ref ALVector2D velocity, ref Vector2D impulse, ref Vector2D point, |
---|
132 | ref Scalar massInv, ref Scalar inertiaInv) |
---|
133 | { |
---|
134 | velocity.Linear.X -= impulse.X * massInv; |
---|
135 | velocity.Linear.Y -= impulse.Y * massInv; |
---|
136 | velocity.Angular -= (inertiaInv * (point.X * impulse.Y - point.Y * impulse.X)); |
---|
137 | } |
---|
138 | } |
---|
139 | } |
---|