Реализация вращения через кватернион. Где ошибка? Пытаюсь реализовать вращение точки. Задача такая: есть молекула, для каждого атома три координаты. У этой молекулы есть группа -C-O-H. Нужно развернуть -O-H на определенный угол вокруг оси C-O. Написал реализацию на java, на простых координатах всё отлично работает, на реальных данных не работает. Помогите, пожалуйста, найти ошибку. (В main-классе закомментированны данные, которые не работают)Main-classpublic class App2
/**
* @param args the command line arguments
*/
public static void main(String[] args) {

// Vector carbon = new Vector(0.864, -8.606, -0.389);
// Vector oxygen = new Vector(-0.495, -8.775, -4.434);
// Vector hydrogen = new Vector(-0.682, -9.652, -0.809);

Vector carbon = new Vector(0, 0, 0);
Vector oxygen = new Vector(2, 2, 2);
Vector hydrogen = new Vector(4, 1, 1);


double angle = Math.PI/2;

// Создаем вектор, вокруг которого будем вращать точку, и нормализуем его
Vector direction = oxygen.residual(carbon);
direction = direction.normalize();
// На основе направляющего вектора и угла вращения создаем кватернион и нормализуем его
Quaternion q = new Quaternion(angle, direction);
q = q.normalize();
// Тут вектор, который будем вращать
Vector rotate = hydrogen.residual(carbon);
// Вращаем
rotate = rotate.transform(q);
rotate = rotate.sum(carbon);

System.out.println(rotate);

}

}Quaternion.javapublic class Quaternion {

double w;
double i;
double j;
double k;

public Quaternion(double angle, Vector vecDir) {
this.w = Math.cos(angle/2);
i = vecDir.x*Math.sin(angle/2);
j = vecDir.y*Math.sin(angle/2);
k = vecDir.z*Math.sin(angle/2);
}

public Quaternion(double w, double i, double j, double k) {
this.w = w;
this.i = i;
this.j = j;
this.k = k;
}

public Quaternion multiply(Quaternion q) {
double nw = w*q.w - i*q.i - j*q.j - k*q.k;
double ni = w*q.i + i*q.w + j*q.k - k*q.j;
double nj = w*q.j - i*q.k + j*q.w + k*q.i;
double nk = w*q.k + i*q.j - j*q.i + k*q.w;
return new Quaternion(nw, ni, nj, nk);
}

public Quaternion multiplyToVector(Vector v) {
Quaternion q = new Quaternion(0, v.x, v.y, v.z);
return multiply(q);
}

public Quaternion normalize() {
double len = Math.sqrt(w*w + i*i + j*j + k*k);
return new Quaternion(w/len, i/len, j/len, k/len);
}

public Quaternion invert() {
Quaternion res = new Quaternion(w, -i, -j, -k);
return res;
}

public Vector toVector() {
return new Vector(i, j, k);
}


}Vector.javaclass Vector {

double x;
double y;
double z;

public Vector(double x, double y, double z) {
this.x = x;
this.y = y;
this.z = z;
}

// Действуем по формуле v' = qvq^(-1)
public Vector transform(Quaternion q) {
Quaternion r = q.multiplyToVector(this);
r = r.multiply(q.invert().normalize());
return r.toVector();
}
@Override
public String toString() {
return String.format("%2f %2f %2f", x, y, z);
}

public Vector normalize() {
double len = Math.sqrt(x*x + y*y + z*z);
return new Vector(x/len, y/len, z/len);
}

public Vector sum(Vector v) {
return new Vector(v.x+x, v.y+y, v.z+z);
}

public Vector residual(Vector v) {
return new Vector(x-v.x, y-v.y, z-v.z);
}

}

Предыдущий
вопрос Следующий
вопрос