-
Notifications
You must be signed in to change notification settings - Fork 13
Expand file tree
/
Copy pathVectorUtils.cs
More file actions
289 lines (265 loc) · 13.5 KB
/
Copy pathVectorUtils.cs
File metadata and controls
289 lines (265 loc) · 13.5 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
// VectorUtils.cs, 07.11.2019
// Copyright (C) Dominic Beger 07.11.2019
using System;
using System.Diagnostics;
namespace SharpMath.Geometry
{
/// <summary>
/// Defines static methods and extensions for working with <see cref="IVector" /> instances.
/// </summary>
public static class VectorUtils
{
/// <summary>
/// Adds two <see cref="IVector" /> instances.
/// </summary>
/// <param name="firstVector">The first <see cref="IVector" />.</param>
/// <param name="secondVector">The second <see cref="IVector" />.</param>
/// <returns>The resulting <see cref="IVector" />.</returns>
public static T Add<T>(this T firstVector, T secondVector) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < resultVector.Dimension; ++i)
resultVector[i] = firstVector[i] + secondVector[i];
return resultVector;
}
/// <summary>
/// Calculates the angle between two <see cref="IVector" /> instances.
/// </summary>
/// <param name="first">The first <see cref="IVector" />.</param>
/// <param name="second">The second <see cref="IVector" />.</param>
/// <returns>The angle between the current and the specified <see cref="IVector" /> instance.</returns>
/// <exception cref="InvalidOperationException">Cannot calculate the angle between two vectors, if one is zero.</exception>
public static double Angle<T>(this T first, T second) where T : IVector
{
if (first.IsZero || second.IsZero)
throw new InvalidOperationException("Cannot calculate the angle between two vectors, if one is zero.");
return Math.Acos(first.DotProduct(second) / (first.Magnitude * second.Magnitude));
}
/// <summary>
/// Represents an <see cref="IVector" /> as a horizontal <see cref="IMatrix" /> whose column count is equal to its
/// dimension.
/// </summary>
/// <returns>The <see cref="IVector" /> represented as horizontal <see cref="IMatrix" />.</returns>
public static TOut AsHorizontalMatrix<TOut>(this IVector vector) where TOut : IMatrix, new()
{
var matrix = new TOut();
if (matrix.RowCount != 1 && matrix.ColumnCount != vector.Dimension)
throw new ArgumentException(
$"Type parameter TOut is not an adequate IMatrix-type as its constraints do not fit those of the resulting matrix. The constraints must be 1x{vector.Dimension}.");
for (uint i = 0; i < vector.Dimension; ++i)
matrix[0, i] = vector[i];
return matrix;
}
/// <summary>
/// Represents an <see cref="IVector" /> as a vertical <see cref="IMatrix" /> whose row count is equal to its
/// dimension.
/// </summary>
/// <returns>The <see cref="IVector" /> represented as vertical <see cref="IMatrix" />.</returns>
public static TOut AsVerticalMatrix<TOut>(this IVector vector) where TOut : IMatrix, new()
{
var matrix = new TOut();
if (matrix.RowCount != vector.Dimension && matrix.ColumnCount != 1)
throw new ArgumentException(
$"Type parameter TOut is not an adequate IMatrix-type as its constraints do not fit those of the resulting matrix. The constraints must be {vector.Dimension}x1.");
for (uint i = 0; i < vector.Dimension; ++i)
matrix[i, 0] = vector[i];
return matrix;
}
/// <summary>
/// Determines whether two <see cref="IVector" /> instances are orthogonal to each other, or not.
/// </summary>
/// <param name="first">The first <see cref="IVector" />.</param>
/// <param name="second">The second <see cref="IVector" />.</param>
/// <returns><c>true</c>, if the <see cref="IVector" /> instances are orthogonal to each other, otherwise <c>false</c>.</returns>
public static bool CheckForOrthogonality<T>(this T first, T second) where T : IVector
{
return !first.IsZero && !second.IsZero &&
FloatingNumber.AreApproximatelyEqual(DotProduct(first, second), 0);
}
/// <summary>
/// Determines whether two <see cref="IVector" /> instances are orthonormal to each other, or not.
/// </summary>
/// <param name="first">The first <see cref="IVector" />.</param>
/// <param name="second">The second <see cref="IVector" />.</param>
/// <returns><c>true</c>, if the <see cref="IVector" /> instances are orthonormal to each other, otherwise <c>false</c>.</returns>
public static bool CheckForOrthonormality<T>(this T first, T second) where T : IVector
{
return first.CheckForOrthogonality(second) && first.IsNormalized && second.IsNormalized;
}
/// <summary>
/// Determines whether two <see cref="IVector" /> instances are parallel to each other, or not.
/// </summary>
/// <param name="first">The first <see cref="IVector" />.</param>
/// <param name="second">The second <see cref="IVector" />.</param>
/// <returns><c>true</c> if the <see cref="IVector" /> instances are parallel to each other, otherwise <c>false</c>.</returns>
public static bool CheckForParallelism<T>(this T first, T second) where T : IVector
{
if (first.IsZero || second.IsZero)
return false;
double firstResult = 0;
for (uint i = 0; i < first.Dimension; ++i)
if (i == 0)
{
firstResult = second[i] / first[i];
}
else
{
if (!FloatingNumber.AreApproximatelyEqual(second[i] / first[i], firstResult))
return false;
}
return true;
}
/// <summary>
/// Creates a new object that is a copy of the current instance.
/// </summary>
/// <returns>
/// A new object that is a copy of this instance.
/// </returns>
public static T Clone<T>(this T vector) where T : IVector, new()
{
var cloneVector = new T();
for (uint i = 0; i < vector.Dimension; ++i)
cloneVector[i] = vector[i];
return cloneVector;
}
/// <summary>
/// Converts an <see cref="IVector" /> into an <see cref="IVector" /> of another dimension.
/// </summary>
/// <typeparam name="TOut">The <see cref="IVector" /> type that the <see cref="IVector" /> should be converted to.</typeparam>
/// <returns>The <see cref="IVector" /> converted into the given type.</returns>
public static TOut Convert<TOut>(this IVector vector) where TOut : IVector, new()
{
var resultVector = new TOut();
if (resultVector.Dimension == vector.Dimension)
Debug.Print(
$"Vector conversion method (Vector{vector.Dimension}.To<T>()) is currently used to convert a vector into one of the same dimension. Please check if this has been your intention.");
for (uint i = 0; i < Math.Min(vector.Dimension, resultVector.Dimension); ++i)
resultVector[i] = vector[i];
return resultVector;
}
/// <summary>
/// Calculates the distance between two <see cref="IVector" /> instances that are the position <see cref="IVector" />s
/// of two points.
/// </summary>
/// <param name="source">The source <see cref="IVector" />.</param>
/// <param name="target">The target <see cref="IVector" />.</param>
/// <returns>The distance between the two <see cref="IVector" /> instances.</returns>
public static double Distance<T>(this T source, T target) where T : IVector, new()
{
return Subtract(source, target).Magnitude;
}
/// <summary>
/// Divides a <see cref="IVector" /> by multipling it with the reciprocal of the scalar.
/// </summary>
/// <param name="vector">The <see cref="IVector" />.</param>
/// <param name="scalar">The scalar whose reciprocal will be calculated.</param>
/// <returns>The resulting <see cref="IVector" />.</returns>
public static T Divide<T>(this T vector, double scalar) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < resultVector.Dimension; ++i)
resultVector[i] = vector[i] * (1 / scalar);
return resultVector;
}
/// <summary>
/// Calculates the dot product of two <see cref="IVector" /> instances.
/// </summary>
/// <param name="first">The first <see cref="IVector" />.</param>
/// <param name="second">The second <see cref="IVector" />.</param>
/// <returns>The calculated scalar as a <see cref="double" />.</returns>
public static double DotProduct<T>(this T first, T second) where T : IVector
{
double result = 0;
for (uint i = 0; i < first.Dimension; ++i)
result += first[i] * second[i];
return result;
}
/// <summary>
/// Linearly interpolates between two <see cref="IVector" /> instances.
/// </summary>
/// <param name="current">The source point.</param>
/// <param name="target">The target point.</param>
/// <param name="interpolant">The fraction.</param>
/// <returns>The position <see cref="IVector" /> of the new point.</returns>
public static T Lerp<T>(this T current, T target, double interpolant) where T : IVector, new()
{
if (interpolant > 1)
interpolant = 1;
else if (interpolant < 0)
interpolant = 0;
return LerpUnclamped(current, target, interpolant);
}
/// <summary>
/// Linearly interpolates between two <see cref="IVector" /> instances.
/// </summary>
/// <param name="current">The source point.</param>
/// <param name="target">The target point.</param>
/// <param name="interpolant">The interpolant factor.</param>
/// <returns>The position <see cref="IVector" /> of the new point.</returns>
public static T LerpUnclamped<T>(this T current, T target, double interpolant) where T : IVector, new()
{
// source + (target - source) * fraction
return Add(current, Multiply(Subtract(target, current), interpolant));
}
/// <summary>
/// Moves the source point in a straight line towards a target point by adding the given distance delta and returns
/// its new position.
/// </summary>
/// <param name="source">The source point.</param>
/// <param name="target">The target point.</param>
/// <param name="maxDistanceDelta">The distance delta that this source point is moved by in all directions.</param>
/// <returns>The position <see cref="IVector" /> of the new point.</returns>
public static T MoveTowards<T>(this T source, T target, double maxDistanceDelta) where T : IVector, new()
{
return source.LerpUnclamped(target, maxDistanceDelta / source.Distance(target));
}
/// <summary>
/// Multiplies a <see cref="IVector" /> with a specified scalar.
/// </summary>
/// <param name="vector">The <see cref="IVector" />.</param>
/// <param name="scalar">The scalar.</param>
/// <returns>The resulting <see cref="IVector" />.</returns>
public static T Multiply<T>(this T vector, double scalar) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < resultVector.Dimension; ++i)
resultVector[i] = vector[i] * scalar;
return resultVector;
}
/// <summary>
/// Negates the specified <see cref="IVector" />.
/// </summary>
/// <returns>The negated <see cref="IVector" />.</returns>
public static T Negate<T>(this T vector) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < vector.Dimension; ++i)
resultVector[i] = -vector[i];
return resultVector;
}
/// <summary>
/// Calculates the normalized <see cref="IVector" /> of the <see cref="IVector" />.
/// </summary>
/// <returns>The normalized <see cref="IVector" />.</returns>
public static T Normalize<T>(this T vector) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < vector.Dimension; ++i)
resultVector[i] = vector[i] / vector.Magnitude;
return resultVector;
}
/// <summary>
/// Subtracts two <see cref="IVector" /> instances.
/// </summary>
/// <param name="firstVector">The first <see cref="IVector" />.</param>
/// <param name="secondVector">The second <see cref="IVector" />.</param>
/// <returns>The resulting <see cref="IVector" />.</returns>
public static T Subtract<T>(this T firstVector, T secondVector) where T : IVector, new()
{
var resultVector = new T();
for (uint i = 0; i < resultVector.Dimension; ++i)
resultVector[i] = firstVector[i] - secondVector[i];
return resultVector;
}
}
}