LZScene

1
//
2
// This unit is part of the GLScene Engine https://github.com/glscene
3
//
4
{
5
   Cubic spline interpolation functions
6
}
7
unit GLSpline;
8

9
interface
10

11
uses
12
  GLVectorGeometry;
13

14
{$I GLScene.inc}
15

16
type
17

18
   TCubicSplineMatrix = array of array [0..3] of Single;
19

20
   { 3D cubic spline handler class.
21
      This class allows to describe and calculate values of a time-based,
22
      three-dimensionnal cubic spline.
23
      Cubic spline pass through all given points and tangent on point N is
24
      given by the (N-1) to (N+1) vector.
25
      Note : X, Y & Z are actually interpolated independently. }
26
   TCubicSpline = class (TObject)
27
      private
28
          
29
         matX, matY, matZ, matW : TCubicSplineMatrix;
30
         FNb : Integer;
31

32
      public
33
          
34
         { Creates the spline and declares interpolation points.
35
            Time references go from 0 (first point) to nb-1 (last point), the
36
            first and last reference matrices respectively are used when T is
37
            used beyond this range.
38
            Note : "nil" single arrays are accepted, in this case the axis is
39
            disabled and calculus will return 0 (zero) for this component. }
40
         constructor Create(const X, Y, Z, W : PFloatArray; const nb : Integer); {$ifdef CLR}unsafe;{$endif}
41
         destructor Destroy; override;
42

43
         { Calculates X component at time t. }
44
         function SplineX(const t : Single): Single;
45
         { Calculates Y component at time t. }
46
         function SplineY(const t : single): Single;
47
         { Calculates Z component at time t. }
48
         function SplineZ(const t : single): Single;
49
         { Calculates W component at time t. }
50
         function SplineW(const t : single): Single;
51

52
         { Calculates X and Y components at time t. }
53
         procedure SplineXY(const t : single; var X, Y : Single);
54
         { Calculates X, Y and Z components at time t. }
55
         procedure SplineXYZ(const t : single; var X, Y, Z : Single);
56
         { Calculates X, Y, Z and W components at time t. }
57
         procedure SplineXYZW(const t : single; var X, Y, Z, W : Single);
58

59
         { Calculates affine vector at time t. }
60
         function SplineAffineVector(const t : single) : TAffineVector; overload;
61
         { Calculates affine vector at time t. }
62
         procedure SplineAffineVector(const t : single; var vector : TAffineVector); overload;
63
         { Calculates vector at time t. }
64
         function SplineVector(const t : single) : TVector; overload;
65
         { Calculates vector at time t. }
66
         procedure SplineVector(const t : single; var vector : TVector); overload;
67

68
         { Calculates X component slope at time t. }
69
         function SplineSlopeX(const t : Single): Single;
70
         { Calculates Y component slope at time t. }
71
         function SplineSlopeY(const t : single): Single;
72
         { Calculates Z component slope at time t. }
73
         function SplineSlopeZ(const t : single): Single;
74
         { Calculates W component slope at time t. }
75
         function SplineSlopeW(const t : single): Single;
76
         { Calculates the spline slope at time t. }
77
         function SplineSlopeVector(const t : single) : TAffineVector; overload;
78

79
         { Calculates the intersection of the spline with the YZ plane.
80
            Returns True if an intersection was found. }
81
         function SplineIntersecYZ(X: Single; var Y, Z: Single): Boolean;
82
         { Calculates the intersection of the spline with the XZ plane.
83
            Returns True if an intersection was found. }
84
         function SplineIntersecXZ(Y: Single; var X, Z: Single): Boolean;
85
         { Calculates the intersection of the spline with the XY plane.
86
            Returns True if an intersection was found. }
87
         function SplineIntersecXY(Z: Single; var X, Y: Single): Boolean;
88
   end;
89

90
// ------------------------------------------------------------------
91
// ------------------------------------------------------------------
92
// ------------------------------------------------------------------
93
implementation
94
// ------------------------------------------------------------------
95
// ------------------------------------------------------------------
96
// ------------------------------------------------------------------
97

98
// VECCholeskyTriDiagResol
99
//
100
procedure VECCholeskyTriDiagResol(const b : array of Single; const nb : Integer;
101
                                  var Result : array of Single);
102
var
103
   Y, LDiag, LssDiag : array of Single;
104
   i, k, Debut, Fin: Integer;
105
begin
106
   Debut:=0;
107
   Fin:=nb-1;
108
   Assert(Length(b)>0);
109
   SetLength(LDiag, nb);
110
   SetLength(LssDiag, nb-1);
111
   LDiag[Debut]:=1.4142135; // = sqrt(2)
112
   LssDiag[Debut]:=1.0/1.4142135;
113
   for K:=Debut+1 to Fin-1 do begin
114
      LDiag[K]:=Sqrt(4-LssDiag[K-1]*LssDiag[K-1]);
115
      LssDiag[K]:=1.0/LDiag[K];
116
   end;
117
   LDiag[Fin]:=Sqrt(2-LssDiag[Fin-1]*LssDiag[Fin-1]);
118
   SetLength(Y, nb);
119
   Y[Debut]:=B[Debut]/LDiag[Debut];
120
   for I:=Debut+1 to Fin do
121
      Y[I]:=(B[I]-Y[I-1]*LssDiag[I-1])/LDiag[I];
122
   Assert(Length(Result)=nb);
123
   Result[Fin]:=Y[Fin]/LDiag[Fin];
124
   for i:=Fin-1 downto Debut do
125
      Result[I]:=(Y[I]-Result[I+1]*LssDiag[I])/LDiag[I];
126
end;
127

128
// MATInterpolationHermite
129
//
130
procedure MATInterpolationHermite(const ordonnees : PFloatArray; const nb : Integer;
131
                                  var Result : TCubicSplineMatrix); {$ifdef CLR}unsafe;{$endif}
132
var
133
   a, b, c, d : Single;
134
   i, n : Integer;
135
   bb, deriv : array of Single;
136
begin
137
   Result:=nil;
138
   if Assigned(Ordonnees) and (nb>0) then begin
139
      n:=nb-1;
140
      SetLength(bb, nb);
141
      bb[0]:=3*(ordonnees[1]-ordonnees[0]);
142
      bb[n]:=3*(ordonnees[n]-ordonnees[n-1]);
143
      for i:=1 to n-1 do
144
         bb[I]:=3*(ordonnees[I+1]-ordonnees[I-1]);
145
      SetLength(deriv, nb);
146
      VECCholeskyTriDiagResol(bb, nb, deriv);
147
      SetLength(Result, n);
148
      for i:=0 to n-1 do begin
149
         a:=ordonnees[I];
150
         b:=deriv[I];
151
         c:=3*(ordonnees[I+1]-ordonnees[I])-2*deriv[I]-deriv[I+1];
152
         d:=-2*(ordonnees[I+1]-ordonnees[I])+deriv[I]+deriv[I+1];
153
         Result[I][3]:=a+I*(I*(c-I*d)-b);
154
         Result[I][2]:=b+I*(3*I*d-2*c);
155
         Result[I][1]:=c-3*I*d;
156
         Result[I][0]:=d;
157
      end;
158
   end;
159
end;
160

161
// MATValeurSpline
162
//
163
function MATValeurSpline(const spline : TCubicSplineMatrix; const x : Single;
164
                         const nb : Integer) : Single;
165
var
166
   i : Integer;
167
begin
168
   if Length(Spline)>0 then begin
169
      if x<=0 then
170
         i:=0
171
      else if x>nb-1 then
172
         i:=nb-1
173
      else i:=Integer(Trunc(x));
174
      { TODO : the following line looks like a bug... }
175
      if i=(nb-1) then Dec(i);
176
      Result:=((spline[i][0]*x+spline[i][1])*x+spline[i][2])*x+spline[i][3];
177
   end else Result:=0;
178
end;
179

180
// MATValeurSplineSlope
181
//
182
function MATValeurSplineSlope(const spline : TCubicSplineMatrix; const x : Single;
183
                              const nb : Integer) : Single;
184
var
185
   i : Integer;
186
begin
187
   if Length(Spline)>0 then begin
188
      if x<=0 then
189
         i:=0
190
      else if x>nb-1 then
191
         i:=nb-1
192
      else i:=Integer(Trunc(x));
193
      { TODO : the following line looks like a bug... }
194
      if i=(nb-1) then Dec(i);
195
      Result:=(3*spline[i][0]*x+2*spline[i][1])*x+spline[i][2];
196
   end else Result:=0;
197
end;
198

199
// ------------------
200
// ------------------ TCubicSpline ------------------
201
// ------------------
202

203
// Create
204
//
205
constructor TCubicSpline.Create(const X, Y, Z, W: PFloatArray; const nb : Integer); {$ifdef CLR}unsafe;{$endif}
206
begin
207
   inherited Create;
208
   MATInterpolationHermite(X, nb, matX);
209
   MATInterpolationHermite(Y, nb, matY);
210
   MATInterpolationHermite(Z, nb, matZ);
211
   MATInterpolationHermite(W, nb, matW);
212
   FNb:=nb;
213
end;
214

215
// Destroy
216
//
217
destructor TCubicSpline.Destroy;
218
begin
219
   inherited Destroy;
220
end;
221

222
// SplineX
223
//
224
function TCubicSpline.SplineX(const t : single): Single;
225
begin
226
   Result:=MATValeurSpline(MatX, t, FNb);
227
end;
228

229
// SplineY
230
//
231
function TCubicSpline.SplineY(const t : single): Single;
232
begin
233
   Result:=MATValeurSpline(MatY, t, FNb);
234
end;
235

236
// SplineZ
237
//
238
function TCubicSpline.SplineZ(const t : single): Single;
239
begin
240
   Result:=MATValeurSpline(MatZ, t, FNb);
241
end;
242

243
// SplineW
244
//
245
function TCubicSpline.SplineW(const t : single): Single;
246
begin
247
   Result:=MATValeurSpline(MatW, t, FNb);
248
end;
249

250
// SplineXY
251
//
252
procedure TCubicSpline.SplineXY(const t : single; var X, Y : Single);
253
begin
254
   X:=MATValeurSpline(MatX, T, FNb);
255
   Y:=MATValeurSpline(MatY, T, FNb);
256
end;
257

258
// SplineXYZ
259
//
260
procedure TCubicSpline.SplineXYZ(const t : single; var X, Y, Z : Single);
261
begin
262
   X:=MATValeurSpline(MatX, T, FNb);
263
   Y:=MATValeurSpline(MatY, T, FNb);
264
   Z:=MATValeurSpline(MatZ, T, FNb);
265
end;
266

267
// SplineXYZW
268
//
269
procedure TCubicSpline.SplineXYZW(const t : single; var X, Y, Z, W : Single);
270
begin
271
   X:=MATValeurSpline(MatX, T, FNb);
272
   Y:=MATValeurSpline(MatY, T, FNb);
273
   Z:=MATValeurSpline(MatZ, T, FNb);
274
   W:=MATValeurSpline(MatW, T, FNb);
275
end;
276

277
// SplineAffineVector
278
//
279
function TCubicSpline.SplineAffineVector(const t : single) : TAffineVector;
280
begin
281
   Result.V[0]:=MATValeurSpline(MatX, t, FNb);
282
   Result.V[1]:=MATValeurSpline(MatY, t, FNb);
283
   Result.V[2]:=MATValeurSpline(MatZ, t, FNb);
284
end;
285

286
// SplineAffineVector
287
//
288
procedure TCubicSpline.SplineAffineVector(const t : single; var vector : TAffineVector);
289
begin
290
   vector.V[0]:=MATValeurSpline(MatX, t, FNb);
291
   vector.V[1]:=MATValeurSpline(MatY, t, FNb);
292
   vector.V[2]:=MATValeurSpline(MatZ, t, FNb);
293
end;
294

295
// SplineVector
296
//
297
function TCubicSpline.SplineVector(const t : single) : TVector;
298
begin
299
   Result.V[0]:=MATValeurSpline(MatX, t, FNb);
300
   Result.V[1]:=MATValeurSpline(MatY, t, FNb);
301
   Result.V[2]:=MATValeurSpline(MatZ, t, FNb);
302
   Result.V[3]:=MATValeurSpline(MatW, t, FNb);
303
end;
304

305
// SplineVector
306
//
307
procedure TCubicSpline.SplineVector(const t : single; var vector : TVector);
308
begin
309
   vector.V[0]:=MATValeurSpline(MatX, t, FNb);
310
   vector.V[1]:=MATValeurSpline(MatY, t, FNb);
311
   vector.V[2]:=MATValeurSpline(MatZ, t, FNb);
312
   vector.V[3]:=MATValeurSpline(MatW, t, FNb);
313
end;
314

315
// SplineSlopeX
316
//
317
function TCubicSpline.SplineSlopeX(const t : Single): Single;
318
begin
319
   Result:=MATValeurSplineSlope(MatX, t, FNb);
320
end;
321

322
// SplineSlopeY
323
//
324
function TCubicSpline.SplineSlopeY(const t : single): Single;
325
begin
326
   Result:=MATValeurSplineSlope(MatY, t, FNb);
327
end;
328

329
// SplineSlopeZ
330
//
331
function TCubicSpline.SplineSlopeZ(const t : single): Single;
332
begin
333
   Result:=MATValeurSplineSlope(MatZ, t, FNb);
334
end;
335

336
// SplineSlopeW
337
//
338
function TCubicSpline.SplineSlopeW(const t : single): Single;
339
begin
340
   Result:=MATValeurSplineSlope(MatW, t, FNb);
341
end;
342

343
// SplineSlopeVector
344
//
345
function TCubicSpline.SplineSlopeVector(const t : single) : TAffineVector;
346
begin
347
   Result.V[0]:=MATValeurSplineSlope(MatX, t, FNb);
348
   Result.V[1]:=MATValeurSplineSlope(MatY, t, FNb);
349
   Result.V[2]:=MATValeurSplineSlope(MatZ, t, FNb);
350
end;
351

352
// SplineIntersecYZ
353
//
354
function TCubicSpline.SplineIntersecYZ(X: Single; var Y, Z: Single): Boolean;
355
var
356
   Sup, Inf, Mid : Single;
357
   SSup, Sinf, Smid : Single;
358
begin
359
   Result:=False;
360

361
   Sup:=FNb;
362
   Inf:=0.0;
363

364
   Ssup:=SplineX(Sup);
365
   Sinf:=SplineX(Inf);
366
   if SSup>Sinf then begin
367
      if (SSup<X) or (Sinf>X) then Exit;
368
      while Abs(SSup-Sinf)>1e-4 do begin
369
         Mid:=(Sup+Inf)*0.5;
370
         SMid:=SplineX(Mid);
371
         if X<SMid then begin
372
            SSup:=SMid;
373
            Sup:=Mid;
374
         end else begin
375
            Sinf:=SMid;
376
            Inf:=Mid;
377
         end;
378
      end;
379
      Y:=SplineY((Sup+Inf)*0.5);
380
      Z:=SplineZ((Sup+Inf)*0.5);
381
   end else begin
382
      if (Sinf<X) or (SSup>X) then Exit;
383
      while Abs(SSup-Sinf)>1e-4 do begin
384
         Mid:=(Sup+Inf)*0.5;
385
         SMid:=SplineX(Mid);
386
         if X<SMid then begin
387
            Sinf:=SMid;
388
            Inf:=Mid;
389
         end else begin
390
            SSup:=SMid;
391
            Sup:=Mid;
392
         end;
393
      end;
394
      Y:=SplineY((Sup+Inf)*0.5);
395
      Z:=SplineZ((Sup+Inf)*0.5);
396
   end;
397
   Result:=True;
398
end;
399

400
// SplineIntersecXZ
401
//
402
function TCubicSpline.SplineIntersecXZ(Y: Single; var X, Z: Single): Boolean;
403
var
404
   Sup, Inf, Mid : Single;
405
   SSup, Sinf, Smid : Single;
406
begin
407
   Result:=False;
408

409
   Sup:=FNb;
410
   Inf:=0.0;
411

412
   Ssup:=SplineY(Sup);
413
   Sinf:=SplineY(Inf);
414
   if SSup>Sinf then begin
415
      if (SSup<Y) or (Sinf>Y) then Exit;
416
      while Abs(SSup-Sinf)>1e-4 do begin
417
         Mid:=(Sup+Inf)*0.5;
418
         SMid:=SplineY(Mid);
419
         if Y<SMid then begin
420
            SSup:=SMid;
421
            Sup:=Mid;
422
         end else begin
423
            Sinf:=SMid;
424
            Inf:=Mid;
425
         end;
426
      end;
427
      X:=SplineX((Sup+Inf)*0.5);
428
      Z:=SplineZ((Sup+Inf)*0.5);
429
   end else begin
430
      if (Sinf<Y) or (SSup>Y) then Exit;
431
      while Abs(SSup-Sinf)>1e-4 do begin
432
         Mid:=(Sup+Inf)*0.5;
433
         SMid:=SplineY(Mid);
434
         if Y<SMid then begin
435
            Sinf:=SMid;
436
            Inf:=Mid;
437
         end else begin
438
            SSup:=SMid;
439
            Sup:=Mid;
440
         end;
441
      end;
442
      X:=SplineX((Sup+Inf)*0.5);
443
      Z:=SplineZ((Sup+Inf)*0.5);
444
   end;
445
   Result:=True;
446
end;
447

448
// SplineIntersecXY
449
//
450
function TCubicSpline.SplineIntersecXY(Z: Single; var X, Y: Single): Boolean;
451
var
452
   Sup, Inf, Mid : Single;
453
   SSup, Sinf, Smid : Single;
454
begin
455
   Result:=False;
456

457
   Sup:=FNb;
458
   Inf:=0.0;
459

460
   Ssup:=SplineZ(Sup);
461
   Sinf:=SplineZ(Inf);
462
   if SSup>Sinf then begin
463
      if (SSup<Z) or (Sinf>Z) then Exit;
464
      while Abs(SSup-Sinf)>1e-4 do begin
465
         Mid:=(Sup+Inf)*0.5;
466
         SMid:=SplineZ(Mid);
467
         if Z<SMid then begin
468
            SSup:=SMid;
469
            Sup:=Mid;
470
         end else begin
471
            Sinf:=SMid;
472
            Inf:=Mid;
473
         end;
474
      end;
475
      X:=SplineX((Sup+Inf)*0.5);
476
      Y:=SplineY((Sup+Inf)*0.5);
477
   end else begin
478
      if (Sinf<Z) or (SSup>Z) then Exit;
479
      while Abs(SSup-Sinf)>1e-4 do begin
480
         Mid:=(Sup+Inf)*0.5;
481
         SMid:=SplineZ(Mid);
482
         if Z<SMid then begin
483
            Sinf:=SMid;
484
            Inf:=Mid;
485
         end else begin
486
            SSup:=SMid;
487
            Sup:=Mid;
488
         end;
489
      end;
490
      X:=SplineX((Sup+Inf)*0.5);
491
      Y:=SplineY((Sup+Inf)*0.5);
492
   end;
493
   Result:=True;
494
end;
495

496
end.
497