1 #include "Vector3DBlock.h" 2 #include "Array.h" 3 4 using std::vector; 5 6 namespace ProtoMol { 7 8 //_________________________________________________________________ Vector3DBlock 9 boundingbox(Vector3D & minbb,Vector3D & maxbb) const10 void Vector3DBlock::boundingbox(Vector3D& minbb, Vector3D& maxbb) const{ 11 if(size() <= 0){ 12 minbb = Vector3D(Constant::MAXREAL,Constant::MAXREAL,Constant::MAXREAL); 13 maxbb = Vector3D(-Constant::MAXREAL,-Constant::MAXREAL,-Constant::MAXREAL); 14 } 15 16 minbb = (*this)[0]; 17 maxbb = (*this)[0]; 18 19 const unsigned int count = size(); 20 for(unsigned int i=1;i<count;++i){ 21 if((*this)[i].x < minbb.x) 22 minbb.x = (*this)[i].x; 23 else if((*this)[i].x > maxbb.x) 24 maxbb.x = (*this)[i].x; 25 if((*this)[i].y < minbb.y) 26 minbb.y = (*this)[i].y; 27 else if((*this)[i].y > maxbb.y) 28 maxbb.y = (*this)[i].y; 29 if((*this)[i].z < minbb.z) 30 minbb.z = (*this)[i].z; 31 else if((*this)[i].z > maxbb.z) 32 maxbb.z = (*this)[i].z; 33 } 34 } 35 sum() const36 Vector3D Vector3DBlock::sum() const{ 37 38 if(empty()) 39 return Vector3D(0.0,0.0,0.0); 40 41 // Loop through all the atoms and remove the motion of center of mass 42 // using an advanced method -- Kahan's magic addition algorithm to get 43 // rid of round-off errors: Scientific Computing pp34. 44 45 Vector3D sum((*this)[0]); 46 Vector3D tempC(0.0,0.0,0.0); 47 for (unsigned int i=1; i<size(); i++) { 48 Vector3D tempX((*this)[i]); 49 Vector3D tempY(tempX - tempC); 50 Vector3D tempT(sum + tempY); 51 tempC = (tempT - sum) - tempY; 52 sum = tempT; 53 } 54 return sum; 55 } 56 57 fitplane(Vector3D & normal,Real & d,Real & err,int limit) const58 bool Vector3DBlock::fitplane(Vector3D& normal, Real& d, Real& err, int limit) const{ 59 60 // Clear return values 61 normal = Vector3D(0.0,0.0,0.0); 62 d = 0.0; 63 err = 0.0; 64 65 // Oh no ... 66 if(size() < 2) 67 return false; 68 69 // Special case, just a perfect plain 70 if(size() == 3){ 71 normal = ((*this)[0]-(*this)[1]).cross((*this)[2]-(*this)[1]); 72 d = -normal.dot((*this)[0]); 73 } 74 75 // General case with more than three points ... 76 else { 77 int n = 4; 78 int m = size(); 79 Array<Real,2> v(ArraySizes((unsigned int)n+1)((unsigned int)n+1)); 80 Array<Real,2> a(ArraySizes((unsigned int)m+1)((unsigned int)n+1)); 81 vector<Real> rv1(n+1); 82 vector<Real> w(n+1); 83 84 85 int flag,i,its,j,jj,k,l=0,nm=0; 86 Real anorm,c,f,g,h,s,scale,x,y,z; 87 for(unsigned int i=0;i<size();i++){ 88 a[i+1][1] = (*this)[i].x; 89 a[i+1][2] = (*this)[i].y; 90 a[i+1][3] = (*this)[i].z; 91 a[i+1][4] = 1.0; 92 } 93 94 // Singular Value Decomposition 95 g=scale=anorm=0.0; 96 97 for (i=1;i<=n;i++) { 98 l=i+1; 99 rv1[i]=scale*g; 100 g=s=scale=0.0; 101 if (i <= m) { 102 for (k=i;k<=m;k++) 103 scale += fabs(a[k][i]); 104 if (scale) { 105 for (k=i;k<=m;k++) { 106 a[k][i] /= scale; 107 s += a[k][i]*a[k][i]; 108 } 109 f=a[i][i]; 110 g = -sign(sqrt(s),f); 111 h=f*g-s; 112 a[i][i]=f-g; 113 for (j=l;j<=n;j++) { 114 for (s=0.0,k=i;k<=m;k++) 115 s += a[k][i]*a[k][j]; 116 f=s/h; 117 for (k=i;k<=m;k++) 118 a[k][j] += f*a[k][i]; 119 } 120 for (k=i;k<=m;k++) 121 a[k][i] *= scale; 122 } 123 } 124 w[i]=scale *g; 125 g=s=scale=0.0; 126 if (i <= m && i != n) { 127 for (k=l;k<=n;k++) 128 scale += fabs(a[i][k]); 129 if (scale) { 130 for (k=l;k<=n;k++) { 131 a[i][k] /= scale; 132 s += a[i][k]*a[i][k]; 133 } 134 f=a[i][l]; 135 g = -sign(sqrt(s),f); 136 h=f*g-s; 137 a[i][l]=f-g; 138 for (k=l;k<=n;k++) 139 rv1[k]=a[i][k]/h; 140 for (j=l;j<=m;j++) { 141 for (s=0.0,k=l;k<=n;k++) 142 s += a[j][k]*a[i][k]; 143 for (k=l;k<=n;k++) 144 a[j][k] += s*rv1[k]; 145 } 146 for (k=l;k<=n;k++) 147 a[i][k] *= scale; 148 } 149 } 150 anorm=std::max(anorm,(fabs(w[i])+fabs(rv1[i]))); 151 } 152 for (i=n;i>=1;i--) { 153 if (i < n) { 154 if (g) { 155 for (j=l;j<=n;j++) 156 v[j][i]=(a[i][j]/a[i][l])/g; 157 for (j=l;j<=n;j++) { 158 for (s=0.0,k=l;k<=n;k++) 159 s += a[i][k]*v[k][j]; 160 for (k=l;k<=n;k++) 161 v[k][j] += s*v[k][i]; 162 } 163 } 164 for (j=l;j<=n;j++) 165 v[i][j]=v[j][i]=0.0; 166 } 167 v[i][i]=1.0; 168 g=rv1[i]; 169 l=i; 170 } 171 for (i=std::min(m,n);i>=1;i--) { 172 l=i+1; 173 g=w[i]; 174 for (j=l;j<=n;j++) 175 a[i][j]=0.0; 176 if (g) { 177 g=1.0/g; 178 for (j=l;j<=n;j++) { 179 for (s=0.0,k=l;k<=m;k++) 180 s += a[k][i]*a[k][j]; 181 f=(s/a[i][i])*g; 182 for (k=i;k<=m;k++) 183 a[k][j] += f*a[k][i]; 184 } 185 for (j=i;j<=m;j++) 186 a[j][i] *= g; 187 } 188 else { 189 for (j=i;j<=m;j++) 190 a[j][i]=0.0; 191 } 192 a[i][i]+=1.0; 193 } 194 for (k=n;k>=1;k--) { 195 for (its=1;its<=limit;its++) { 196 flag=1; 197 for (l=k;l>=1;l--) { 198 nm=l-1; 199 if ((double)(fabs(rv1[l])+anorm) == anorm) { 200 flag=0; 201 break; 202 } 203 if ((double)(fabs(w[nm])+anorm) == anorm) 204 break; 205 } 206 if (flag) { 207 c=0.0; 208 s=1.0; 209 for (i=l;i<=k;i++) { 210 f=s*rv1[i]; 211 rv1[i]=c*rv1[i]; 212 if ((double)(fabs(f)+anorm) == anorm) 213 break; 214 g=w[i]; 215 h=norm(f,g); 216 w[i]=h; 217 h=1.0/h; 218 c=g*h; 219 s = -f*h; 220 for (j=1;j<=m;j++) { 221 y=a[j][nm]; 222 z=a[j][i]; 223 a[j][nm]=y*c+z*s; 224 a[j][i]=z*c-y*s; 225 } 226 } 227 } 228 z=w[k]; 229 if (l == k) { 230 if (z < 0.0) { 231 w[k] = -z; 232 for (j=1;j<=n;j++) 233 v[j][k] = -v[j][k]; 234 } 235 break; 236 } 237 if (its == limit) 238 return false; 239 x=w[l]; 240 nm=k-1; 241 y=w[nm]; 242 g=rv1[nm]; 243 h=rv1[k]; 244 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y); 245 g=norm(f,1.0); 246 f=((x-z)*(x+z)+h*((y/(f+sign(g,f)))-h))/x; 247 c=s=1.0; 248 for (j=l;j<=nm;j++) { 249 i=j+1; 250 g=rv1[i]; 251 y=w[i]; 252 h=s*g; 253 g=c*g; 254 z=norm(f,h); 255 rv1[j]=z; 256 c=f/z; 257 s=h/z; 258 f=x*c+g*s; 259 g = g*c-x*s; 260 h=y*s; 261 y *= c; 262 for (jj=1;jj<=n;jj++) { 263 x=v[jj][j]; 264 z=v[jj][i]; 265 v[jj][j]=x*c+z*s; 266 v[jj][i]=z*c-x*s; 267 } 268 z=norm(f,h); 269 w[j]=z; 270 if (z) { 271 z=1.0/z; 272 c=f*z; 273 s=h*z; 274 } 275 f=c*g+s*y; 276 x=c*y-s*g; 277 for (jj=1;jj<=m;jj++) { 278 y=a[jj][j]; 279 z=a[jj][i]; 280 a[jj][j]=y*c+z*s; 281 a[jj][i]=z*c-y*s; 282 } 283 } 284 rv1[l]=0.0; 285 rv1[k]=f; 286 w[k]=x; 287 } 288 } 289 290 // Find smallest singular value 291 j = -1; 292 for(i=1;i<=n;i++){ 293 if(j < 0 || w[j] > w[i]){ 294 j = i; 295 } 296 } 297 298 // Get solution 299 normal.x = v[1][j]; 300 normal.y = v[2][j]; 301 normal.z = v[3][j]; 302 d = v[4][j]; 303 } 304 305 // Nomralize normal and plane constant, z >= 0.0 306 Real nd = normal.norm(); 307 if(nd == 0.0){ 308 normal = Vector3D(0.0,0.0,0.0); 309 d = 0.0; 310 err = 0.0; 311 return false; 312 } 313 314 if(normal.z < 0.0){ 315 normal = -normal; 316 d = -d; 317 } 318 normal = normal/nd; 319 d = d/nd; 320 321 // The error 322 for(unsigned int i=0;i<size();i++) 323 err = power<2>((*this)[i].dot(normal)+d); 324 err = sqrt(err); 325 326 return true; 327 328 } 329 330 } 331