| 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)) ; |
