18 #include <emmintrin.h>
22 #define RAD2DEG 180./M_PI
31 xy->v=_mm_setzero_pd();
32 zx->v=_mm_setzero_pd();
38 xy->v=_mm_setr_pd(x,y);
39 zx->v=_mm_setr_pd(z,x);
58 void SetXY(
double x,
double y){
59 xy->v=_mm_setr_pd(x,y);
61 void SetXYZ(
double x,
double y,
double z){
62 xy->v=_mm_setr_pd(x,y);
63 zx->v=_mm_setr_pd(z,x);
65 void SetMagThetaPhi(
double p,
double theta,
double phi){
67 double pt=my_p*
sin(theta);
68 xy->d[0]=zx->d[1]=pt*cos(phi);
70 zx->d[0]=my_p*cos(theta);
73 void SetPhi(
double phi){
75 xy->d[0]=zx->d[1]=pt*cos(phi);
89 double operator () (
int ind)
const {
106 double x()
const {
return xy->d[0];};
107 double y()
const {
return xy->d[1];};
108 double z()
const {
return zx->d[0];};
109 double X()
const {
return xy->d[0];};
110 double Y()
const {
return xy->d[1];};
111 double Z()
const {
return zx->d[0];};
112 double Px()
const {
return xy->d[0];};
113 double Py()
const {
return xy->d[1];};
114 double Pz()
const {
return zx->d[0];};
116 double CosTheta()
const{
118 return r == 0.0 ? 1.0 : z()/r;
120 double Theta()
const{
121 return acos(CosTheta());
124 return atan2(
y(),
x());
127 double Perp2()
const{
128 return (xy->d[0]*xy->d[0]+xy->d[1]*xy->d[1]);
132 return (
sqrt(Perp2()));
134 double Pt()
const{
return Perp();};
137 return(xy->d[0]*xy->d[0]+xy->d[1]*xy->d[1]+zx->d[0]*zx->d[0]);
141 return (
sqrt(Mag2()));
153 struct dvec1 &yz=p[0];
154 yz.v[0]=_mm_setr_pd(
y(),z());
155 yz.v[1]=_mm_setr_pd(v1.y(),v1.z());
156 return DVector3(_mm_sub_pd(_mm_mul_pd(yz.v[0],v1.zx->v),
157 _mm_mul_pd(zx->v,yz.v[1])),
158 _mm_sub_pd(_mm_mul_pd(xy->v,yz.v[1]),
159 _mm_mul_pd(yz.v[0],v1.xy->v))
165 double xx=
x()<0.0 ? -
x() :
x();
166 double yy=
y()<0.0 ? -
y() :
y();
167 double zz= z()<0.0 ? -z() : z();
179 scale=_mm_set1_pd(c/Mag());
180 xy->v=_mm_mul_pd(xy->v,scale);
181 zx->v=_mm_mul_pd(zx->v,scale);
186 bool operator==(
const DVector3 &v1)
const {
187 return ((
x()==v1.x() &&
y()==v1.y() && z()==v1.z())?
true :
false);
190 bool operator!=(
const DVector3 &v1)
const{
191 return ((
x()!=v1.x() ||
y()!=v1.y() || z()!=v1.z())?
true :
false);
204 xy->v=_mm_add_pd(xy->v,v1.xy->v);
205 zx->v=_mm_add_pd(zx->v,v1.zx->v);
211 xy->v=_mm_sub_pd(xy->v,v1.xy->v);
212 zx->v=_mm_sub_pd(zx->v,v1.zx->v);
218 return DVector3(_mm_add_pd(GetVxy(),v2.GetVxy()),
219 _mm_add_pd(GetVzx(),v2.GetVzx()));
223 return DVector3(_mm_sub_pd(GetVxy(),v2.GetVxy()),
224 _mm_sub_pd(GetVzx(),v2.GetVzx()));
233 scale=_mm_set1_pd(c);
234 xy->v=_mm_mul_pd(GetVxy(),scale);
235 zx->v=_mm_mul_pd(GetVzx(),scale);
243 zero=_mm_set1_pd(0.);
244 return DVector3(_mm_sub_pd(zero,xy->v),_mm_sub_pd(zero,zx->v));
252 sa=_mm_set1_pd(
sin(a));
253 ca=_mm_set1_pd(cos(a));
254 xy->v=_mm_add_pd(_mm_mul_pd(ca,xy->v),
255 _mm_mul_pd(sa,_mm_setr_pd(-
y(),
x())));
265 sa=_mm_set1_pd(
sin(a));
266 ca=_mm_set1_pd(cos(a));
272 yz->v=_mm_add_pd(_mm_mul_pd(ca,_mm_setr_pd(
y(),z())),
273 _mm_mul_pd(sa,_mm_setr_pd(-z(),
y())));
284 sa=_mm_set1_pd(
sin(a));
285 ca=_mm_set1_pd(cos(a));
286 zx->v=_mm_add_pd(_mm_mul_pd(ca,zx->v),
287 _mm_mul_pd(sa,_mm_setr_pd(-
x(),z())));
313 __m128d &one_minus_ca=p[3];
314 __m128d &sa_zero=p[4];
315 __m128d &ca_zero=p[5];
316 __m128d &zero_sa=p[6];
317 __m128d &zero_ca=p[7];
333 one_minus_ca=_mm_set1_pd(1.-ca);
334 sa_zero=_mm_setr_pd(sa,0.);
335 ca_zero=_mm_setr_pd(ca,0.);
336 zero_sa=_mm_setr_pd(0.,sa);
337 zero_ca=_mm_setr_pd(0.,ca);
338 scale=_mm_set1_pd(1./vmag);
339 dxdy=_mm_mul_pd(scale,v.GetVxy());
340 dzdx=_mm_mul_pd(scale,v.GetVzx());
341 dxdx=_mm_mul_pd(scale,_mm_set1_pd(v.x()));
342 dydy=_mm_mul_pd(scale,_mm_set1_pd(v.y()));
343 dzdz=_mm_mul_pd(scale,_mm_set1_pd(v.z()));
344 AD=_mm_add_pd(_mm_add_pd(ca_zero,
345 _mm_mul_pd(one_minus_ca,
346 _mm_mul_pd(dxdy,dxdx))),
347 _mm_mul_pd(zero_sa,dzdz));
348 BE=_mm_sub_pd(_mm_add_pd(zero_ca,
349 _mm_mul_pd(one_minus_ca,
350 _mm_mul_pd(dxdy,dydy))),
351 _mm_mul_pd(sa_zero,dzdz));
352 CF=_mm_sub_pd(_mm_add_pd(_mm_mul_pd(one_minus_ca,
353 _mm_mul_pd(dxdy,dzdz)),
354 _mm_mul_pd(sa_zero,dydy)),
355 _mm_mul_pd(zero_sa,dxdx));
356 IA=_mm_sub_pd(_mm_add_pd(zero_ca,
357 _mm_mul_pd(one_minus_ca,
358 _mm_mul_pd(dzdx,dxdx))),
359 _mm_mul_pd(sa_zero,dydy));
360 JB=_mm_sub_pd(_mm_add_pd(_mm_mul_pd(sa_zero,dxdx),
361 _mm_mul_pd(one_minus_ca,
362 _mm_mul_pd(dzdx,dydy))),
363 _mm_mul_pd(zero_sa,dzdz));
364 KC=_mm_add_pd(_mm_add_pd(ca_zero,
365 _mm_mul_pd(one_minus_ca,
366 _mm_mul_pd(dzdx,dzdz))),
367 _mm_mul_pd(zero_sa,dydy));
369 xy->v=_mm_add_pd(_mm_add_pd(_mm_mul_pd(AD,xx),
370 _mm_mul_pd(BE,yy)), _mm_mul_pd(CF,zz));
371 zx->v=_mm_add_pd(_mm_add_pd(_mm_mul_pd(IA,xx),
372 _mm_mul_pd(JB,yy)), _mm_mul_pd(KC,zz));
379 double Dot(
const DVector3 &v)
const{
380 return (
x()*v.x()+
y()*v.y()+z()*v.z());
384 double Angle(
const DVector3 &v)
const{
386 double v2mag=v.Mag();
387 if (v2mag>0. && v1mag > 0.){
388 double costheta=Dot(v)/v1mag/v2mag;
390 if (costheta>1.)
return 0.;
391 if (costheta<-1.)
return M_PI;
392 return acos(costheta);
399 cout <<
"DVector3 (x,y,z)=("<<setprecision(5)<<
x() <<
","
400 << setprecision(5) <<
y() <<
","<<setprecision(5) << z() <<
"), "
401 <<
"(rho,theta,phi)=("<<setprecision(5) << Mag() <<
","
402 << setprecision(5) <<
RAD2DEG*Theta() <<
","
403 << setprecision(5) <<
RAD2DEG*Phi() <<
")"
409 __m128d GetVxy()
const {
return xy->v;};
410 __m128d GetVzx()
const {
return zx->v;};
418 union dvec*
const zx;
425 scale=_mm_set1_pd(c);
427 _mm_mul_pd(v1.GetVzx(),scale));
433 scale=_mm_set1_pd(c);
435 _mm_mul_pd(v1.GetVzx(),scale));
DMatrix2x1 operator*(const double c, const DMatrix2x1 &M)
#define ALIGNED_16_BLOCK_PTR(TYPE, NUM, PTR)
DVector2S operator+(const DVector2S &vec1, const DVector2S &vec2)
#define ALIGNED_16_BLOCK_WITH_PTR(TYPE, NUM, PTR)
DVector2S operator-(const DVector2S &vec1, const DVector2S &vec2)
#define ALIGNED_16_BLOCK(TYPE, NUM, PTR)