00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 using namespace std;
00013
00014 #include "MucGeomSvc/MucGeometron.h"
00015
00016 MucGeometron::MucGeometron()
00017 {
00018
00019 }
00020
00021 MucGeometron::~MucGeometron()
00022 {
00023
00024 }
00025
00026 bool
00027 MucGeometron::GetIntersectionLinePlane(const HepPoint3D pLine,
00028 const Hep3Vector vectLine,
00029 const HepPlane3D plane,
00030 HepPoint3D& cross)
00031 {
00032
00033
00034 HepPoint3D linePoint = pLine;
00035 Hep3Vector lineDir = vectLine;
00036
00037 HepPoint3D planePoint = plane.point();
00038 Hep3Vector planeDir(plane.a(), plane.b(), plane.c());
00039
00040 lineDir.setMag(1.0);
00041 planeDir.setMag(1.0);
00042
00043
00044 HepPoint3D basePoint = planePoint - linePoint;
00045
00046 double distance, denominator;
00047
00048 denominator = planeDir.dot(lineDir);
00049
00050 if( denominator != 0 ) {
00051
00052 distance =
00053 (planeDir.dot(basePoint)) / denominator;
00054 cross = linePoint + lineDir * distance;
00055 return 1;
00056 }
00057 else {
00058
00059
00060 return 0;
00061 }
00062 }
00063
00064
00065 bool
00066 MucGeometron::GetIntersectionLinePlaneWithSigma(const HepPoint3D pLine,
00067 const Hep3Vector vectLine,
00068 const HepPoint3D pLineSigma,
00069 const Hep3Vector vectLineSigma,
00070 const HepPlane3D plane,
00071 HepPoint3D& cross,
00072 HepPoint3D& crossSigma)
00073 {
00074 HepPoint3D linePoint = pLine;
00075 Hep3Vector lineDir = vectLine;
00076
00077 HepPoint3D planePoint = plane.point();
00078 Hep3Vector planeDir(plane.a(), plane.b(), plane.c());
00079
00080 lineDir.setMag(1.0);
00081 planeDir.setMag(1.0);
00082 HepPoint3D basePoint = planePoint - linePoint;
00083
00084 double distance, denominator;
00085
00086 denominator = planeDir.dot(lineDir);
00087 if( denominator != 0 ) {
00088 distance =
00089 (planeDir.dot(basePoint)) / denominator;
00090 cross = linePoint + lineDir * distance;
00091
00092
00093
00094 double x0 = pLine.x();
00095 double y0 = pLine.y();
00096 double z0 = pLine.z();
00097
00098 double vx = vectLine.x();
00099 double vy = vectLine.y();
00100 double vz = vectLine.z();
00101
00102 double dx0 = pLineSigma.x();
00103 double dy0 = pLineSigma.y();
00104 double dz0 = pLineSigma.z();
00105
00106 double dvx = vectLineSigma.x();
00107 double dvy = vectLineSigma.y();
00108 double dvz = vectLineSigma.z();
00109
00110 double pa = plane.a();
00111 double pb = plane.b();
00112 double pc = plane.c();
00113
00114 double Const1 = planeDir.dot(planePoint);
00115 double Const2 = pa*vx + pb*vy + pc*vz;
00116 double Const3 = pa*x0 + pb*y0 + pc*z0;
00117
00118
00119 double sigmaX_1 = dx0 * dx0 * ( pb * vy + pc * vz ) / Const2;
00120 double sigmaX_2 = (-1) * dy0 * dy0 * pb * vx / Const2;
00121 double sigmaX_3 = (-1) * dz0 * dz0 * pc * vx / Const2;
00122 double sigmaX_4 = dvx * dvx * ( Const1 - Const3) * ( pb * vy + pc * vz) / Const2 / Const2;
00123 double sigmaX_5 = (-1) * dvy * dvy * vx * ( Const1 - Const3) * pb / Const2 / Const2;
00124 double sigmaX_6 = (-1) * dvz * dvz * vx * ( Const1 - Const3) * pc / Const2 / Const2;
00125
00126 double sigmaX = sigmaX_1 + sigmaX_2 + sigmaX_3 + sigmaX_4 + sigmaX_5 + sigmaX_6;
00127
00128 double sigmaY_1 = (-1) * dx0 * dx0 * pa * vy / Const2;
00129 double sigmaY_2 = dy0 * dy0 * ( 1 - pb * vy / Const2);
00130 double sigmaY_3 = (-1) * dz0 * dz0 * pc * vy / Const2;
00131 double sigmaY_4 = (-1) * dvx * dvx * ( Const1 - Const3) * pa * vy / Const2 / Const2;
00132 double sigmaY_5 = dvy * dvy * ( Const1 - Const3) * ( pa * vx + pc * vz) / Const2 / Const2;
00133 double sigmaY_6 = (-1) * dvz * dvz * ( Const1 - Const3) * pc * vy / Const2 / Const2;
00134
00135 double sigmaY = sigmaY_1 + sigmaY_2 + sigmaY_3 + sigmaY_4 + sigmaY_5 + sigmaY_6;
00136
00137 double sigmaZ_1 = (-1) * dx0 * dx0 * pa * vz / Const2;
00138 double sigmaZ_2 = (-1) * dy0 * dy0 * pb * vz / Const2;
00139 double sigmaZ_3 = dz0 * dz0 * ( 1 - pc * vz / Const2);
00140 double sigmaZ_4 = (-1) * dvx * dvx * ( Const1 - Const3) * pa * vz / Const2 / Const2;
00141 double sigmaZ_5 = (-1) * dvy * dvy * ( Const1 - Const3) * pb * vz / Const2 / Const2;
00142 double sigmaZ_6 = dvz * dvz * ( Const1 - Const3) * ( pa * vx + pb * vy) / Const2 / Const2;
00143
00144 double sigmaZ = sigmaZ_1 + sigmaZ_2 + sigmaZ_3 + sigmaZ_4 + sigmaZ_5 + sigmaZ_6;
00145
00146
00147 HepPoint3D c1(sqrt(abs(sigmaX)), sqrt(abs(sigmaY)), sqrt(abs(sigmaZ)));
00148 crossSigma = c1;
00149 return 1;
00150 }
00151 else {
00152 return 0;
00153 }
00154 }
00155
00156 bool
00157 MucGeometron::GetIntersectionQuadPlaneLocal(const int part,
00158 const int orient,
00159 const float a,
00160 const float b,
00161 const float c,
00162 const HepPlane3D plane,
00163 HepPoint3D& cross1,
00164 HepPoint3D& cross2)
00165 {
00166
00167 if(a == 0) {
00168
00169 return 0;
00170
00171 }
00172
00173 HepPoint3D planePoint = plane.point();
00174 Hep3Vector planeDir(plane.a(), plane.b(), plane.c());
00175
00176 float A, B, C, D;
00177 A = plane.a(); B = plane.b(); C = plane.c();
00178 D = planeDir.dot(planePoint);
00179
00180
00181
00182
00183
00184 float a1 = B*a;
00185 float b1 = B*b + A;
00186 float c1 = B*c - D;
00187
00188 float b2sub4ac = b1*b1 - 4*a1*c1;
00189
00190
00191
00192 float x1, x2, y1, y2, z1, z2;
00193 if(abs(a1)>10E-10){
00194 if(b2sub4ac>=0)
00195 {
00196 x1 = (-b1+sqrt(b2sub4ac))/(2*a1);
00197 x2 = (-b1-sqrt(b2sub4ac))/(2*a1);
00198 y1 = a*x1*x1 + b*x1 + c;
00199 y2 = a*x2*x2 + b*x2 + c;
00200
00201
00202 cross1.set(x1, y1, z1);
00203 cross2.set(x2, y2, z2);
00204 return 1;
00205 }
00206 else{
00207
00208 cross1.set(-9999,-9999,-9999);
00209 cross2.set(-9999,-9999,-9999);
00210 return 0;
00211 }
00212 }else{
00213 x1 = -c1/b1;
00214 y1 = a*x1*x1 + b*x1 + c;
00215
00216 cross1.set(x1, y1, z1);
00217 cross2.set(x1, y1, z1);
00218
00219 return 1;
00220
00221 }
00222
00223
00224 }
00225
00226
00227
00228 bool
00229 MucGeometron::GetIntersectionQuadPlane(const HepPoint3D pLine,
00230 const float vy,
00231 const float y0,
00232 const float a,
00233 const float b,
00234 const float c,
00235 const HepPlane3D plane,
00236 HepPoint3D& cross1,
00237 HepPoint3D& cross2)
00238 {
00239
00240
00241
00242
00243
00244
00245
00246 HepPoint3D planePoint = plane.point();
00247 Hep3Vector planeDir(plane.a(), plane.b(), plane.c());
00248
00249 float A, B, C, D;
00250 A = plane.a(); B = plane.b(); C = plane.c();
00251 D = planeDir.dot(planePoint);
00252
00253
00254
00255 float a1 = (B+C/vy)*a;
00256 float b1 = (B+C/vy)*b + A;
00257 float c1 = (B+C/vy)*c - C/y0 - D;
00258
00259 float b2sub4ac = b1*b1 - 4*a1*c1;
00260
00261 float x1, x2, y1, y2, z1, z2;
00262 if(abs(a1)>10E-10){
00263 if(b2sub4ac>=0)
00264 {
00265 x1 = (-b1+sqrt(b2sub4ac))/(2*a1);
00266 x2 = (-b1-sqrt(b2sub4ac))/(2*a1);
00267 y1 = a*x1*x1 + b*x1 + c;
00268 y2 = a*x2*x2 + b*x2 + c;
00269 z1 = (y1 - y0)/vy;
00270 z2 = (y2 - y0)/vy;
00271
00272
00273 cross1.set(x1, y1, z1);
00274 cross2.set(x2, y2, z2);
00275 return 1;
00276 }
00277 else{
00278
00279 cross1.set(-9999,-9999,-9999);
00280 cross2.set(-9999,-9999,-9999);
00281 return 0;
00282 }
00283 }else{
00284 x1 = -c1/b1;
00285 y1 = a*x1*x1 + b*x1 + c;
00286 z1 = (y1 - y0)/vy;
00287
00288 cross1.set(x1, y1, z1);
00289 cross2.set(x1, y1, z1);
00290
00291 return 1;
00292
00293 }
00294
00295 }