125 |
|
Mat3x3d orderTensor(0.0); |
126 |
|
for (std::vector<std::pair<StuntDouble*, StuntDouble*> >::iterator j = sdPairs_.begin(); j != sdPairs_.end(); ++j) { |
127 |
|
Vector3d vec = j->first->getPos() - j->second->getPos(); |
128 |
+ |
currentSnapshot_->wrapVector(vec); |
129 |
|
vec.normalize(); |
130 |
|
orderTensor +=outProduct(vec, vec); |
131 |
|
} |
132 |
|
|
133 |
|
orderTensor /= sdPairs_.size(); |
134 |
< |
orderTensor -= 1.0/3.0 * Mat3x3d::identity(); |
134 |
> |
orderTensor -= (RealType)(1.0/3.0) * Mat3x3d::identity(); |
135 |
|
|
136 |
|
Vector3d eigenvalues; |
137 |
|
Mat3x3d eigenvectors; |
138 |
|
Mat3x3d::diagonalize(orderTensor, eigenvalues, eigenvectors); |
139 |
|
|
140 |
|
int which; |
141 |
< |
double maxEval = 0.0; |
141 |
> |
RealType maxEval = 0.0; |
142 |
|
for(int k = 0; k< 3; k++){ |
143 |
|
if(fabs(eigenvalues[k]) > maxEval){ |
144 |
|
which = k; |
145 |
|
maxEval = fabs(eigenvalues[k]); |
146 |
|
} |
147 |
|
} |
148 |
< |
double p2 = 1.5 * maxEval; |
148 |
> |
RealType p2 = 1.5 * maxEval; |
149 |
|
|
150 |
|
//the eigen vector is already normalized in SquareMatrix3::diagonalize |
151 |
|
Vector3d director = eigenvectors.getColumn(which); |
153 |
|
director.negate(); |
154 |
|
} |
155 |
|
|
156 |
< |
double angle = 0.0; |
156 |
> |
RealType angle = 0.0; |
157 |
|
for (std::vector<std::pair<StuntDouble*, StuntDouble*> >::iterator j = sdPairs_.begin(); j != sdPairs_.end(); ++j) { |
158 |
|
Vector3d vec = j->first->getPos() - j->second->getPos(); |
159 |
+ |
currentSnapshot_->wrapVector(vec); |
160 |
|
vec.normalize(); |
161 |
|
|
162 |
|
angle += acos(dot(vec, director)) ; |