98 |
|
Vector3d DirectionalRCorrFunc::calcCorrVals(int frame1, int frame2, StuntDouble* sd1, StuntDouble* sd2) { |
99 |
|
Vector3d r1 = sd1->getPos(frame1); |
100 |
|
Vector3d r2 = sd2->getPos(frame2); |
101 |
– |
Vector3d u1 = sd1->getA(frame1).getColumn(2); |
101 |
|
|
102 |
+ |
// The lab frame vector corresponding to the body-fixed |
103 |
+ |
// z-axis is simply the second column of A.transpose() |
104 |
+ |
// or, identically, the second row of A itself. |
105 |
+ |
|
106 |
+ |
Vector3d u1 = sd1->getA(frame1).getRow(2); |
107 |
+ |
RealType u1l = u1.length(); |
108 |
+ |
|
109 |
|
RealType rsq = (r2-r1).lengthSquare(); |
110 |
< |
RealType rpar = dot( r2-r1, u1); |
110 |
> |
RealType rpar = dot( (r2-r1), u1)/u1l; |
111 |
|
RealType rpar2 = rpar*rpar; |
112 |
|
RealType rperp2 = rsq - rpar2; |
113 |
|
|