PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusImplicitSplineForce.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) John SH Baxter, Robarts Research Institute. All rights reserved.
4 See License.txt for details.
5 =========================================================================*/
6 
7 #include "PlusConfigure.h"
8 
10 #include "vtkMatrix4x4.h"
11 #include "vtkObjectFactory.h"
12 #include <iostream>
13 #include <math.h>
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <time.h>
17 
18 //----------------------------------------------------------------------------
19 
21 
22 //----------------------------------------------------------------------------
24 {
25  // Constant for sigmoid function
26  this->gammaSigmoid = 2;
27  this->scaleForce = 20.0;
28  this->SplineKnots = "knot3DHeart.txt";
29  this->ControlPoints = " ";
30 
31 }
32 
33 //----------------------------------------------------------------------------
34 void vtkPlusImplicitSplineForce::PrintSelf(ostream& os, vtkIndent indent)
35 {
36  this->Superclass::PrintSelf(os, indent.GetNextIndent());
37  os << indent.GetNextIndent() << "Gamma Sigmoid: " << this->gammaSigmoid << endl;
38 }
39 
40 //----------------------------------------------------------------------------
42 {
43 
44 }
45 
46 //----------------------------------------------------------------------------
47 void vtkPlusImplicitSplineForce::SetInput(char * controlPnt)
48 {
49  this->ControlPoints = controlPnt;
50  ReadFileControlPoints(controlPnt);
51 }
52 
53 //----------------------------------------------------------------------------
55 {
57 
58  // Read B-spline control points (coefficients) from file
59  if (splineId <= 0 || splineId > 20)
60  {
61  return;
62  }
63 
64  switch(splineId)
65  {
66  case 1:
67  ReadFileControlPoints("control3DLSHeart01.txt");
68  break;
69  case 2:
70  ReadFileControlPoints("control3DLSHeart02.txt");
71  break;
72  case 3:
73  ReadFileControlPoints("control3DLSHeart03.txt");
74  break;
75  case 4:
76  ReadFileControlPoints("control3DLSHeart04.txt");
77  break;
78  case 5:
79  ReadFileControlPoints("control3DLSHeart05.txt");
80  break;
81  case 6:
82  ReadFileControlPoints("control3DLSHeart06.txt");
83  break;
84  case 7:
85  ReadFileControlPoints("control3DLSHeart07.txt");
86  break;
87  case 8:
88  ReadFileControlPoints("control3DLSHeart08.txt");
89  break;
90  case 9:
91  ReadFileControlPoints("control3DLSHeart09.txt");
92  break;
93  case 10:
94  ReadFileControlPoints("control3DLSHeart10.txt");
95  break;
96  case 11:
97  ReadFileControlPoints("control3DLSHeart11.txt");
98  break;
99  case 12:
100  ReadFileControlPoints("control3DLSHeart12.txt");
101  break;
102  case 13:
103  ReadFileControlPoints("control3DLSHeart13.txt");
104  break;
105  case 14:
106  ReadFileControlPoints("control3DLSHeart14.txt");
107  break;
108  case 15:
109  ReadFileControlPoints("control3DLSHeart15.txt");
110  break;
111  case 16:
112  ReadFileControlPoints("control3DLSHeart16.txt");
113  break;
114  case 17:
115  ReadFileControlPoints("control3DLSHeart17.txt");
116  break;
117  case 18:
118  ReadFileControlPoints("control3DLSHeart18.txt");
119  break;
120  case 19:
121  ReadFileControlPoints("control3DLSHeart19.txt");
122  break;
123  case 20:
124  ReadFileControlPoints("control3DLSHeart20.txt");
125  break;
126  }
127 }
128 
129 //----------------------------------------------------------------------------
130 int vtkPlusImplicitSplineForce::GenerateForce(vtkMatrix4x4 * transformMatrix, double force[3])
131 {
132  int n=3; //Cubic spline
133  int i,j;
134  int flag = 0;
135  double dSpline, value, deriv, gradient[3];
136 
137  // Calculate distance (dSpline) to B-spline surface
138  dSpline = CalculateDistanceBasis(transformMatrix->GetElement(0,3), transformMatrix->GetElement(1,3), transformMatrix->GetElement(2,3), n, 3);
139 
140  if(dSpline > -1.0)
141  {
142  flag = 1;
143  }
144 
145  CalculateDistanceDerivativeBasis(transformMatrix->GetElement(0,3), transformMatrix->GetElement(1,3), transformMatrix->GetElement(2,3), n, 3, gradient);
146 
147  fnGaussValueDeriv(gammaSigmoid, dSpline, value, deriv);
148 
149  for(i=0; i<3; i++)
150  {
151  force[i] = 0;
152  for(j=0; j<3; j++)
153  {
154  force[i] += gradient[i];
155  }
156  force[i] *= -deriv;
157  }
158 
159  // Set magnitude of the force to the values of potential fields
160  double fMag = sqrt(force[0]*force[0]+force[1]*force[1]+force[2]*force[2]);
161  if(fMag>1e-10)
162  {
163  for(i=0; i<3; i++)
164  {
165  force[i]=value*force[i]/fMag;
166  }
167  }
168 
169  return flag;
170 }
171 
172 //----------------------------------------------------------------------------
173 int vtkPlusImplicitSplineForce::fnSigmoidValueDeriv(double a, double x, double& value, double& deriv)
174 {
175  double tmp = exp(-a*x);
176 
177  value = 1/(1+tmp);
178 
179  if(tmp>1.0e20) // avoid overflow???
180  {
181  deriv = 0;
182  }
183  else
184  {
185  deriv = a*tmp/( (1+tmp)*(1+tmp) );
186  }
187 
188  return 0;
189 }
190 
191 //----------------------------------------------------------------------------
192 int vtkPlusImplicitSplineForce::fnGaussValueDeriv(double a, double x, double& value, double& deriv)
193 {
194  int m=2;
195  a = 2;
196 
197  double tmp = exp(-pow(x*x/(a*a), m));
198 
199  value = 1-tmp;
200 
201  deriv = tmp*2*m*pow(x,2*m-1)/pow(a*a, m);
202 
203  return 0;
204 }
205 
206 //----------------------------------------------------------------------------
208 {
209  gammaSigmoid = gamma;
210 
211  return 0;
212 }
213 
214 //----------------------------------------------------------------------------
215 int vtkPlusImplicitSplineForce::CalculateDistanceDerivativeBasis(double x, double y, double z, int n, int direction, double *gradient)
216 {
217  double u = x;
218  double v = y;
219  double w = z;
220  int Nk;
221  int Iu, Iv, Iw;
222  int i,j,k;
223 
224  Iu = DimKnot_U-1-n;
225  Iv = DimKnot_V-1-n;
226  Iw = DimKnot_W-1-n;
227 
228  // Calculate Iu, Iv, Iw;
229  for(i=0; i<DimKnot_U; i++)
230  {
231  if(u<knot1b[i])
232  {
233  Iu = i-1;
234  break;
235  }
236  }
237  if(Iu<n-1)
238  {
239  Iu = n-1;
240  }
241  else if(Iu>DimKnot_U-1-n)
242  {
243  Iu = DimKnot_U-1-n;
244  }
245 
246  for(i=0; i<DimKnot_V; i++)
247  {
248  if(v<knot2b[i])
249  {
250  Iv = i-1;
251  break;
252  }
253  }
254  if(Iv<n-1)
255  {
256  Iv = n-1;
257  }
258  else if(Iv>DimKnot_V-1-n)
259  {
260  Iv = DimKnot_V-1-n;
261  }
262 
263  for(i=0; i<DimKnot_W; i++)
264  {
265  if(w<knot3b[i])
266  {
267  Iw = i-1;
268  break;
269  }
270  }
271  if(Iw<n-1)
272  {
273  Iw = n-1;
274  }
275  else if(Iw>DimKnot_W-1-n)
276  {
277  Iw = DimKnot_W-1-n;
278  }
279 
280  double Nu[10], Nv[10], Nw[10];
281  double dNu[10], dNv[10], dNw[10];
282  for(i=0; i<=n; i++)
283  {
284  Nk = Iu - 2 + i;
285  Nu[i] = BasisFunction3(Nk, knot1b, u, DimKnot_U-1);
286  dNu[i] = BasisFunction3DerivativeD(Nk, knot1b, u, DimKnot_U-1);
287  }
288  for(i=0; i<=n; i++)
289  {
290  Nk = Iv - 2 + i;
291  Nv[i] = BasisFunction3(Nk, knot2b, v, DimKnot_V-1);
292  dNv[i] = BasisFunction3DerivativeD(Nk, knot2b, v, DimKnot_V-1);
293  }
294  for(i=0; i<=n; i++)
295  {
296  Nk = Iw - 2 + i;
297  Nw[i] = BasisFunction3(Nk, knot3b, w, DimKnot_W-1);
298  dNw[i] = BasisFunction3DerivativeD(Nk, knot3b, w, DimKnot_W-1);
299  }
300 
301  double distDT;
302  distDT = 0;
303  for(k=0; k<=n; k++)
304  {
305  for(i=0; i<=n; i++)
306  {
307  for(j=0; j<=n; j++)
308  {
309  distDT += controlQ3D[Iw-2+k][Iv-2+i][Iu-2+j]*Nw[k]*Nv[i]*dNu[j];
310  }
311  }
312  }
313  gradient[0] = distDT;
314 
315  distDT = 0;
316  for(k=0; k<=n; k++)
317  {
318  for(i=0; i<=n; i++)
319  {
320  for(j=0; j<=n; j++)
321  {
322  distDT += controlQ3D[Iw-2+k][Iv-2+i][Iu-2+j]*Nw[k]*dNv[i]*Nu[j];
323  }
324  }
325  }
326  gradient[1] = distDT;
327 
328  distDT = 0;
329  for(k=0; k<=n; k++)
330  {
331  for(i=0; i<=n; i++)
332  {
333  for(j=0; j<=n; j++)
334  {
335  distDT += controlQ3D[Iw-2+k][Iv-2+i][Iu-2+j]*dNw[k]*Nv[i]*Nu[j];
336  }
337  }
338  }
339  gradient[2] = distDT;
340 
341  return 0;
342 }
343 
344 //----------------------------------------------------------------------------
345 double vtkPlusImplicitSplineForce::BasisFunction3(int k, double *knot, double u, int K)
346 {
347  double Nu = 0;
348  double tmp1, tmp2, tmp3;
349 
350  // If u is outside of knot sequence, return 0;
351  if( u<knot[0] || u>knot[K])
352  {
353  return 0; //???
354  }
355 
356  //exception k=0
357  if(k==0)
358  {
359  if(u<knot[k+1])
360  {
361  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
362  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
363  tmp3 = (knot[k+3]-u)/(knot[k+3]-knot[k])*(u-knot[k])/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
364  Nu = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(tmp1+tmp2) + tmp3;
365  }
366  else if(u<knot[k+2])
367  {
368  tmp1 = (u-knot[k])/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
369  tmp2 = (knot[k+3]-u)/(knot[k+3]-knot[k+1])*(u-knot[k+1])/(knot[k+2]-knot[k+1]);
370  tmp3 = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(knot[k+2]-u)/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
371  Nu = (knot[k+3]-u)/(knot[k+3]-knot[k])*(tmp1+tmp2) + tmp3;
372  }
373  else if(u<knot[k+3])
374  {
375  Nu = (knot[k+3]-u)*(knot[k+3]-u)*(knot[k+3]-u)/( (knot[k+3]-knot[k])*(knot[k+3]-knot[k+1])*(knot[k+3]-knot[k+2]) );
376  }
377  }
378 
379  //exception k=K-2
380  else if(k==K-2)
381  {
382  if(u<=knot[k])
383  {
384  Nu = (u - knot[k-1])*(u - knot[k-1])*(u - knot[k-1])/( (knot[k+2]-knot[k-1])*(knot[k+1]-knot[k-1])*(knot[k]-knot[k-1]) );
385  }
386  else if(u<knot[k+1])
387  {
388  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
389  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
390  Nu = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(tmp1+tmp2);
391  }
392  else if(u<knot[k+2])
393  {
394  tmp3 = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(knot[k+2]-u)/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
395  Nu = tmp3;
396  }
397  }
398 
399  // Normal: 1<=k<=K-3
400  else
401  {
402  if(u<knot[k])
403  {
404  Nu = (u - knot[k-1])*(u - knot[k-1])*(u - knot[k-1])/( (knot[k+2]-knot[k-1])*(knot[k+1]-knot[k-1])*(knot[k]-knot[k-1]) );
405  }
406  else if(u<knot[k+1])
407  {
408  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
409  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
410  tmp3 = (knot[k+3]-u)/(knot[k+3]-knot[k])*(u-knot[k])/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
411  Nu = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(tmp1+tmp2) + tmp3;
412  }
413  else if(u<knot[k+2])
414  {
415  tmp1 = (u-knot[k])/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
416  tmp2 = (knot[k+3]-u)/(knot[k+3]-knot[k+1])*(u-knot[k+1])/(knot[k+2]-knot[k+1]);
417  tmp3 = (u-knot[k-1])/(knot[k+2]-knot[k-1])*(knot[k+2]-u)/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
418  Nu = (knot[k+3]-u)/(knot[k+3]-knot[k])*(tmp1+tmp2) + tmp3;
419  }
420  else if(u<knot[k+3])
421  {
422  Nu = (knot[k+3]-u)*(knot[k+3]-u)*(knot[k+3]-u)/( (knot[k+3]-knot[k])*(knot[k+3]-knot[k+1])*(knot[k+3]-knot[k+2]) );
423  }
424  }
425 
426  return Nu;
427 }
428 
429 //----------------------------------------------------------------------------
430 // Calculate first derivative of cubic B-Spline basis function:
431 double vtkPlusImplicitSplineForce::BasisFunction3DerivativeD(int k, double *knot, double u, int K)
432 {
433  double dNu = 0;
434  double tmp1, tmp2;
435  double dtmp,dtmp1,dtmp2,dtmp3;
436 
437  // If u is outside of knot sequence, return 0;
438  if( u<knot[0] || u>knot[K])
439  {
440  return 0; //???
441  }
442 
443  //exception k=0
444  if(k==0)
445  {
446  if(u<knot[k+1])
447  {
448  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
449  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
450 
451  dtmp1 = ( (knot[k+1]-u) + (u-knot[k-1])*(-1) )/(knot[k+1]-knot[k-1])/(knot[k+1]-knot[k]);
452  dtmp2 = ( (-1)*(u-knot[k]) + (knot[k+2]-u) )/(knot[k+2]-knot[k])/(knot[k+1]-knot[k]);
453 
454  dtmp = (knot[k+3]-knot[k])*(knot[k+2]-knot[k])*(knot[k+1]-knot[k]);
455  dtmp3 = ( (-1)*(u-knot[k])*(u-knot[k]) + (knot[k+3]-u)*2*(u-knot[k]) )/dtmp;
456 
457  dNu = 1/(knot[k+2]-knot[k-1])*(tmp1+tmp2) + (u-knot[k-1])/(knot[k+2]-knot[k-1])*(dtmp1+dtmp2)+dtmp3;
458  }
459  else if(u<knot[k+2])
460  {
461  tmp1 = (u-knot[k])/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
462  tmp2 = (knot[k+3]-u)/(knot[k+3]-knot[k+1])*(u-knot[k+1])/(knot[k+2]-knot[k+1]);
463 
464  dtmp1 = ( (knot[k+2]-u) + (u-knot[k])*(-1) )/(knot[k+2]-knot[k])/(knot[k+2]-knot[k+1]);
465  dtmp2 = ( (-1)*(u-knot[k+1]) + (knot[k+3]-u) )/(knot[k+3]-knot[k+1])/(knot[k+2]-knot[k+1]);
466 
467  dtmp = (knot[k+2]-knot[k-1])*(knot[k+2]-knot[k])*(knot[k+2]-knot[k+1]);
468  dtmp3 = ( (knot[k+2]-u)*(knot[k+2]-u) + (u-knot[k-1])*(-2)*(knot[k+2]-u) )/dtmp;
469 
470  dNu = (-1)/(knot[k+3]-knot[k])*(tmp1+tmp2) + (knot[k+3]-u)/(knot[k+3]-knot[k])*(dtmp1+dtmp2)+dtmp3;
471  }
472  else if(u<knot[k+3])
473  {
474  dNu = (-3)*(knot[k+3]-u)*(knot[k+3]-u)/( (knot[k+3]-knot[k])*(knot[k+3]-knot[k+1])*(knot[k+3]-knot[k+2]) );
475  }
476  }
477 
478  //exception k=K-2
479  else if(k==K-2)
480  {
481  if(u<=knot[k])
482  {
483  dNu = 3*(u - knot[k-1])*(u - knot[k-1])/( (knot[k+2]-knot[k-1])*(knot[k+1]-knot[k-1])*(knot[k]-knot[k-1]) );
484  }
485  else if(u<knot[k+1])
486  {
487  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
488  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
489 
490  dtmp1 = ( (knot[k+1]-u) + (u-knot[k-1])*(-1) )/(knot[k+1]-knot[k-1])/(knot[k+1]-knot[k]);
491  dtmp2 = ( (-1)*(u-knot[k]) + (knot[k+2]-u) )/(knot[k+2]-knot[k])/(knot[k+1]-knot[k]);
492 
493  dNu = 1/(knot[k+2]-knot[k-1])*(tmp1+tmp2) + (u-knot[k-1])/(knot[k+2]-knot[k-1])*(dtmp1+dtmp2);
494  }
495  else if(u<knot[k+2])
496  {
497  dtmp = (knot[k+2]-knot[k-1])*(knot[k+2]-knot[k])*(knot[k+2]-knot[k+1]);
498  dtmp3 = ( (knot[k+2]-u)*(knot[k+2]-u) + (u-knot[k-1])*(-2)*(knot[k+2]-u) )/dtmp;
499 
500  dNu = dtmp3;
501  }
502  }
503  else
504  {
505  if(u<knot[k])
506  {
507  dNu = 3*(u - knot[k-1])*(u - knot[k-1])/( (knot[k+2]-knot[k-1])*(knot[k+1]-knot[k-1])*(knot[k]-knot[k-1]) );
508  }
509  else if(u<knot[k+1])
510  {
511  tmp1 = (u-knot[k-1])/(knot[k+1]-knot[k-1])*(knot[k+1]-u)/(knot[k+1]-knot[k]);
512  tmp2 = (knot[k+2]-u)/(knot[k+2]-knot[k])*(u-knot[k])/(knot[k+1]-knot[k]);
513 
514  dtmp1 = ( (knot[k+1]-u) + (u-knot[k-1])*(-1) )/(knot[k+1]-knot[k-1])/(knot[k+1]-knot[k]);
515  dtmp2 = ( (-1)*(u-knot[k]) + (knot[k+2]-u) )/(knot[k+2]-knot[k])/(knot[k+1]-knot[k]);
516 
517  dtmp = (knot[k+3]-knot[k])*(knot[k+2]-knot[k])*(knot[k+1]-knot[k]);
518  dtmp3 = ( (-1)*(u-knot[k])*(u-knot[k]) + (knot[k+3]-u)*2*(u-knot[k]) )/dtmp;
519 
520  dNu = 1/(knot[k+2]-knot[k-1])*(tmp1+tmp2) + (u-knot[k-1])/(knot[k+2]-knot[k-1])*(dtmp1+dtmp2)+dtmp3;
521  }
522  else if(u<knot[k+2])
523  {
524  tmp1 = (u-knot[k])/(knot[k+2]-knot[k])*(knot[k+2]-u)/(knot[k+2]-knot[k+1]);
525  tmp2 = (knot[k+3]-u)/(knot[k+3]-knot[k+1])*(u-knot[k+1])/(knot[k+2]-knot[k+1]);
526 
527  dtmp1 = ( (knot[k+2]-u) + (u-knot[k])*(-1) )/(knot[k+2]-knot[k])/(knot[k+2]-knot[k+1]);
528  dtmp2 = ( (-1)*(u-knot[k+1]) + (knot[k+3]-u) )/(knot[k+3]-knot[k+1])/(knot[k+2]-knot[k+1]);
529 
530  dtmp = (knot[k+2]-knot[k-1])*(knot[k+2]-knot[k])*(knot[k+2]-knot[k+1]);
531  dtmp3 = ( (knot[k+2]-u)*(knot[k+2]-u) + (u-knot[k-1])*(-2)*(knot[k+2]-u) )/dtmp;
532 
533  dNu = (-1)/(knot[k+3]-knot[k])*(tmp1+tmp2) + (knot[k+3]-u)/(knot[k+3]-knot[k])*(dtmp1+dtmp2)+dtmp3;
534  }
535  else if(u<knot[k+3])
536  {
537  dNu = (-3)*(knot[k+3]-u)*(knot[k+3]-u)/( (knot[k+3]-knot[k])*(knot[k+3]-knot[k+1])*(knot[k+3]-knot[k+2]) );
538  }
539  }
540 
541  return dNu;
542 }
543 
544 //----------------------------------------------------------------------------
545 double vtkPlusImplicitSplineForce::CalculateDistanceBasis(double x, double y, double z, int n, int direction)
546 {
547  double u = x;
548  double v = y;
549  double w = z;
550  int Nk;
551  int Iu, Iv, Iw;
552  int i,j,k;
553 
554  // Calculate Iu, Iv, Iw;
555  Iu = CalculateKnotIu(u, knot1b, DimKnot_U-1, n);
556  Iv = CalculateKnotIu(v, knot2b, DimKnot_V-1, n);
557  Iw = CalculateKnotIu(w, knot3b, DimKnot_W-1, n);
558 
559  //double distance = BasisFunction3(Nk, knot, u, K);
560  double Nu[10], Nv[10], Nw[10];
561  for(i=0; i<=n; i++)
562  {
563  Nk = Iu - 2 + i;
564  Nu[i] = BasisFunction3(Nk, knot1b, u, DimKnot_U-1);
565  }
566  for(i=0; i<=n; i++)
567  {
568  Nk = Iv - 2 + i;
569  Nv[i] = BasisFunction3(Nk, knot2b, v, DimKnot_V-1);
570  }
571  for(i=0; i<=n; i++)
572  {
573  Nk = Iw - 2 + i;
574  Nw[i] = BasisFunction3(Nk, knot3b, w, DimKnot_W-1);
575  }
576 
577  double distT = 0;
578  for(k=0; k<=n; k++)
579  {
580  for(i=0; i<=n; i++)
581  {
582  for(j=0; j<=n; j++)
583  {
584  distT += controlQ3D[Iw-2+k][Iv-2+i][Iu-2+j]*Nw[k]*Nv[i]*Nu[j];
585  }
586  }
587  }
588 
589  return distT;
590 }
591 
592 //----------------------------------------------------------------------------
593 int vtkPlusImplicitSplineForce::CalculateKnotIu(double u, double *knot, int K, int n)
594 {
595  int Iu = K-n;
596 
597  for(int i=0; i<=K; i++)
598  {
599  if(u<knot[i])
600  {
601  Iu = i-1;
602  break;
603  }
604  }
605 
606  if(Iu<n-1)
607  {
608  Iu = n-1;
609  }
610  else if(Iu>K-n)
611  {
612  Iu = K-n;
613  }
614 
615  return Iu;
616 }
617 
618 //----------------------------------------------------------------------------
620 {
621  if( fname.empty() )
622  {
623  return -1;
624  }
625  std::ifstream fpInKnot;
626  fpInKnot.open(fname.c_str());
627 
628  if( !fpInKnot.is_open() )
629  {
630  return -1;
631  }
632 
633  // knot1/2/3
634  for(int k=0; k<=NUM_INTERVALU_S; k++)
635  {
636  fpInKnot >> knot1[k];
637  }
638 
639  for(int k=0; k<=NUM_INTERVALV_S; k++)
640  {
641  fpInKnot >> knot2[k];
642  }
643 
644  for(int k=0; k<=NUM_INTERVALW_S; k++)
645  {
646  fpInKnot >> knot3[k];
647  }
648 
649  // knot1/2/3b
650  for(int k=0; k<=DimKnot_U-1; k++)
651  {
652  fpInKnot >> knot1b[k];
653  }
654  for(int k=0; k<=DimKnot_V-1; k++)
655  {
656  fpInKnot >> knot2b[k];
657  }
658  for(int k=0; k<=DimKnot_W-1; k++)
659  {
660  fpInKnot >> knot3b[k];
661  }
662 
663  fpInKnot.close();
664 
665  return 0;
666 }
667 
668 //----------------------------------------------------------------------------
670 {
671  if( fname.empty() )
672  {
673  return -1;
674  }
675  std::ifstream fpInKnot;
676  fpInKnot.open(fname.c_str());
677 
678  if( !fpInKnot.is_open() )
679  {
680  return -1;
681  }
682 
683  int i,j,k;
684  for(k=0; k<DimCPoint_W; k++)
685  {
686  for(i=0; i<DimCPoint_V; i++)
687  {
688  for(j=0; j<DimCPoint_U; j++)
689  {
690  fpInKnot >> controlQ3D[k][i][j];
691  }
692  }
693  }
694 
695  fpInKnot.close();
696 
697  return 0;
698 }
#define DimKnot_V
int GenerateForce(vtkMatrix4x4 *transformMatrix, double force[3])
#define NUM_INTERVALV_S
double BasisFunction3(int k, double *knot, double u, int K)
double controlQ3D[DimCPoint_W][DimCPoint_V][DimCPoint_U]
#define DimKnot_U
int fnGaussValueDeriv(double a, double x, double &value, double &deriv)
vtkStandardNewMacro(vtkPlusImplicitSplineForce) vtkPlusImplicitSplineForce
for i
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
double knot3[NUM_INTERVALW_S+1]
double CalculateDistanceBasis(double x, double y, double z, int n, int direction)
#define DimCPoint_U
int CalculateDistanceDerivativeBasis(double x, double y, double z, int n, int direction, double *gradient)
int ReadFileControlPoints(const std::string &fname)
#define DimKnot_W
double BasisFunction3DerivativeD(int k, double *knot, double u, int K)
#define DimCPoint_W
#define DimCPoint_V
#define NUM_INTERVALU_S
double knot1[NUM_INTERVALU_S+1]
int x
Definition: phidget22.h:4265
int CalculateKnotIu(double u, double *knot, int K, int n)
const char const char * value
Definition: phidget22.h:5111
int ReadFile3DBSplineKnots(const std::string &fname)
Direction vectors of rods y
Definition: algo3.m:15
#define NUM_INTERVALW_S
double knot2[NUM_INTERVALV_S+1]
int fnSigmoidValueDeriv(double a, double x, double &value, double &deriv)
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
Position vectors of rods v
Definition: algo3.m:14