00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "G4NystromRK4.hh"
00035 #include <iostream>
00036
00038
00040
00041 G4NystromRK4::G4NystromRK4(G4Mag_EqRhs* magEqRhs, G4double distanceConstField)
00042 : G4MagIntegratorStepper(magEqRhs, 6),
00043 m_fEq( magEqRhs ),
00044 m_magdistance( distanceConstField ),
00045 m_cof( 0.0 ),
00046 m_mom( 0.0 ),
00047 m_imom( 0.0 ),
00048 m_cachedMom( false )
00049 {
00050 m_fldPosition[0] = m_iPoint[0] = m_fPoint[0] = m_mPoint[0] = 9.9999999e+99 ;
00051 m_fldPosition[1] = m_iPoint[1] = m_fPoint[1] = m_mPoint[1] = 9.9999999e+99 ;
00052 m_fldPosition[2] = m_iPoint[2] = m_fPoint[2] = m_mPoint[2] = 9.9999999e+99 ;
00053 m_fldPosition[3] = -9.9999999e+99;
00054 m_lastField[0] = m_lastField[1] = m_lastField[2] = 0.0;
00055
00056 m_magdistance2 = distanceConstField*distanceConstField;
00057 }
00058
00060
00062
00063 G4NystromRK4::~G4NystromRK4()
00064 {
00065 }
00066
00068
00070
00071 void
00072 G4NystromRK4::Stepper
00073 (const G4double P[],const G4double dPdS[],G4double Step,G4double Po[],G4double Err[])
00074 {
00075 G4double R[3] = { P[0], P[1] , P[2]};
00076 G4double A[3] = {dPdS[0], dPdS[1], dPdS[2]};
00077
00078 m_iPoint[0]=R[0]; m_iPoint[1]=R[1]; m_iPoint[2]=R[2];
00079
00080 const G4double one_sixth= 1./6.;
00081 G4double S = Step ;
00082 G4double S5 = .5*Step ;
00083 G4double S4 = .25*Step ;
00084 G4double S6 = Step * one_sixth;
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094 G4double K1[3] = { m_imom*dPdS[3], m_imom*dPdS[4], m_imom*dPdS[5] };
00095
00096
00097
00098 G4double p[4] = {R[0]+S5*(A[0]+S4*K1[0]),
00099 R[1]+S5*(A[1]+S4*K1[1]),
00100 R[2]+S5*(A[2]+S4*K1[2]),
00101 P[7] };
00102 getField(p);
00103
00104 G4double A2[3] = {A[0]+S5*K1[0],A[1]+S5*K1[1],A[2]+S5*K1[2]};
00105 G4double K2[3] = {(A2[1]*m_lastField[2]-A2[2]*m_lastField[1])*m_cof,
00106 (A2[2]*m_lastField[0]-A2[0]*m_lastField[2])*m_cof,
00107 (A2[0]*m_lastField[1]-A2[1]*m_lastField[0])*m_cof};
00108
00109 m_mPoint[0]=p[0]; m_mPoint[1]=p[1]; m_mPoint[2]=p[2];
00110
00111
00112
00113 G4double A3[3] = {A[0]+S5*K2[0],A[1]+S5*K2[1],A[2]+S5*K2[2]};
00114 G4double K3[3] = {(A3[1]*m_lastField[2]-A3[2]*m_lastField[1])*m_cof,
00115 (A3[2]*m_lastField[0]-A3[0]*m_lastField[2])*m_cof,
00116 (A3[0]*m_lastField[1]-A3[1]*m_lastField[0])*m_cof};
00117
00118
00119
00120 p[0] = R[0]+S*(A[0]+S5*K3[0]);
00121 p[1] = R[1]+S*(A[1]+S5*K3[1]);
00122 p[2] = R[2]+S*(A[2]+S5*K3[2]);
00123
00124 getField(p);
00125
00126 G4double A4[3] = {A[0]+S*K3[0],A[1]+S*K3[1],A[2]+S*K3[2]};
00127 G4double K4[3] = {(A4[1]*m_lastField[2]-A4[2]*m_lastField[1])*m_cof,
00128 (A4[2]*m_lastField[0]-A4[0]*m_lastField[2])*m_cof,
00129 (A4[0]*m_lastField[1]-A4[1]*m_lastField[0])*m_cof};
00130
00131
00132
00133 Po[0] = P[0]+S*(A[0]+S6*(K1[0]+K2[0]+K3[0]));
00134 Po[1] = P[1]+S*(A[1]+S6*(K1[1]+K2[1]+K3[1]));
00135 Po[2] = P[2]+S*(A[2]+S6*(K1[2]+K2[2]+K3[2]));
00136
00137 m_fPoint[0]=Po[0]; m_fPoint[1]=Po[1]; m_fPoint[2]=Po[2];
00138
00139
00140
00141 Po[3] = A[0]+S6*(K1[0]+K4[0]+2.*(K2[0]+K3[0]));
00142 Po[4] = A[1]+S6*(K1[1]+K4[1]+2.*(K2[1]+K3[1]));
00143 Po[5] = A[2]+S6*(K1[2]+K4[2]+2.*(K2[2]+K3[2]));
00144
00145
00146
00147 Err[3] = S*std::fabs(K1[0]-K2[0]-K3[0]+K4[0]);
00148 Err[4] = S*std::fabs(K1[1]-K2[1]-K3[1]+K4[1]);
00149 Err[5] = S*std::fabs(K1[2]-K2[2]-K3[2]+K4[2]);
00150 Err[0] = S*Err[3] ;
00151 Err[1] = S*Err[4] ;
00152 Err[2] = S*Err[5] ;
00153 Err[3]*= m_mom ;
00154 Err[4]*= m_mom ;
00155 Err[5]*= m_mom ;
00156
00157
00158
00159 G4double normF = m_mom/std::sqrt(Po[3]*Po[3]+Po[4]*Po[4]+Po[5]*Po[5]);
00160 Po [3]*=normF; Po[4]*=normF; Po[5]*=normF;
00161
00162
00163 Po[6]=P[6]; Po[7]=P[7];
00164 }
00165
00166
00168
00170
00171 G4double
00172 G4NystromRK4::DistChord() const
00173 {
00174 G4double ax = m_fPoint[0]-m_iPoint[0];
00175 G4double ay = m_fPoint[1]-m_iPoint[1];
00176 G4double az = m_fPoint[2]-m_iPoint[2];
00177 G4double dx = m_mPoint[0]-m_iPoint[0];
00178 G4double dy = m_mPoint[1]-m_iPoint[1];
00179 G4double dz = m_mPoint[2]-m_iPoint[2];
00180 G4double d2 = (ax*ax+ay*ay+az*az) ;
00181
00182 if(d2!=0.) {
00183 G4double ds = (ax*dx+ay*dy+az*dz)/d2;
00184 dx -= (ds*ax) ;
00185 dy -= (ds*ay) ;
00186 dz -= (ds*az) ;
00187 }
00188 return std::sqrt(dx*dx+dy*dy+dz*dz);
00189 }
00190
00192
00194
00195 void
00196 G4NystromRK4::ComputeRightHandSide(const G4double P[],G4double dPdS[])
00197 {
00198 G4double P4vec[4]= { P[0], P[1], P[2], P[7] };
00199 getField(P4vec);
00200 m_mom = std::sqrt(P[3]*P[3]+P[4]*P[4]+P[5]*P[5]) ;
00201 m_imom = 1./m_mom ;
00202 m_cof = m_fEq->FCof()*m_imom ;
00203 m_cachedMom = true ;
00204 dPdS[0] = P[3]*m_imom ;
00205 dPdS[1] = P[4]*m_imom ;
00206 dPdS[2] = P[5]*m_imom ;
00207 dPdS[3] = m_cof*(P[4]*m_lastField[2]-P[5]*m_lastField[1]) ;
00208 dPdS[4] = m_cof*(P[5]*m_lastField[0]-P[3]*m_lastField[2]) ;
00209 dPdS[5] = m_cof*(P[3]*m_lastField[1]-P[4]*m_lastField[0]) ;
00210 }