Сфера сфера столкновения (C++)

Question

Я реализую некоторый базовый движок физики 3D. Для столкновения сферы с сферой я следую за этим tutorial. У меня проблемы с двумя движущимися сферами. Я думаю, может быть, какая-то проблема с тем, как я нахожу «укороченный скорость»

bool collidingDSmove(Sphere sphere){ 
    // Early Escape test: if the length of the movevec is less 
    // than distance between the centers of these circles minus 
    // their radii, there's no way they can hit. 

В моих векторов скорости тестовой сцены являются (-2, -4, -2) и (1,2,1). Поэтому «shortVel» становится фактически больше, чем исходные скорости.

vec3 shortVel = sphere.velocity.substract(velocity); 
    vec3 fromAtoBCenter = position.substract(sphere.position); 
    float distSquare = fromAtoBCenter.getLengthSquare(); 
    float sumRadii = (radius + sphere.radius); 
    distSquare -= sumRadii*sumRadii; 
    if (shortVel.getLengthSquare() < distSquare){ 
     return false; 
    } 

    // Normalize the movevec 
    vec3 N = shortVel.normalize(); 

    // Find C, the vector from the center of the moving 
    // circle A to the center of B 
    vec3 C = sphere.position.substract(position); 

    // D = N . C = ||C|| * cos(angle between N and C) 
    float D = N.dot(C); 

    // Another early escape: Make sure that A is moving 
    // towards B! If the dot product between the movevec and 
    // B.center - A.center is less that or equal to 0, 
    // A isn't isn't moving towards B 
    if (D <= 0){ 
     return false; 
    } 
    // Find the length of the vector C 
    float lengthCSquare = C.getLengthSquare(); 

    float F = (lengthCSquare)-(D * D); 

    // Escape test: if the closest that A will get to B 
    // is more than the sum of their radii, there's no 
    // way they are going collide 
    float sumRadiiSquared = sumRadii * sumRadii; 
    if (F >= sumRadiiSquared){ 
     return false; 
    } 

    // We now have F and sumRadii, two sides of a right triangle. 
    // Use these to find the third side, sqrt(T) 
    double T = sumRadiiSquared - F; 

    // If there is no such right triangle with sides length of 
    // sumRadii and sqrt(f), T will probably be less than 0. 
    // Better to check now than perform a square root of a 
    // negative number. 
    if (T < 0){ 
     return false; 
    } 

    // Therefore the distance the circle has to travel along 
    // movevec is D - sqrt(T) 
    float distance = D - sqrt(T); 

    // Get the magnitude of the movement vector 
    float mag = velocity.getLength(); 

    // Finally, make sure that the distance A has to move 
    // to touch B is not greater than the magnitude of the 
    // movement vector. 
    if (mag < distance){ 
     return false; 
    } 

// *** сумма не между 0 и 1

float amount = shortVel.normalize().getLength()/velocity.getLength(); 
    // Set the length of the movevec so that the circles will just 
    // touch 
    velocity = velocity.normalize().times(amount); 
    sphere.velocity = sphere.velocity.normalize().times(amount); 

    return true; 
} 

Мой класс vec3 выглядит следующим образом:

class vec3 { 
public: 
    float x; float y; float z; 
    vec3() : x(0), y(0), z(0) { } 
    vec3 substract(vec3 v){ 
     vec3 sub; 
     sub.x = x - v.x; 
     sub.y = y - v.y; 
     sub.z = z - v.z; 
     return sub; 
    } 
float getLength() { 
     return sqrt(x*x + y*y + z*z); 
    } 
float getLengthSquare() { 
     return x*x + y*y + z*z; 
    } 
vec3 normalize(){ 
     vec3 n; 
     n.x = x/getLength(); 
     n.y = y/getLength(); 
     n.z = z/getLength(); 
     return n; 
    } 
float dot(vec3 v) { 
     return x*v.x + y*v.y + z*v.z; 
    } 
} 

Не могли бы вы объяснить мне, что я делаю неправильно здесь ?

Это моя функция обновления. Где deltaT это время, прошедшее между 2 кадра

void updateVelocity(double deltaT){ 
     velocity.x = velocity.x + acceleration.x*deltaT; 
     velocity.y = velocity.y + acceleration.y*deltaT; 
     velocity.z = velocity.z + acceleration.z*deltaT; 
    } 
    void updatePosition(double deltaT){ 
     position.x = position.x + velocity.x * deltaT + 0.5 * acceleration.x * deltaT * deltaT; 
     position.y = position.y + velocity.y * deltaT + 0.5 * acceleration.y * deltaT * deltaT; 
     position.z = position.z + velocity.z * deltaT + 0.5 * acceleration.z * deltaT * deltaT; 
    } 
    void update(double deltaT){ 
     updateVelocity(deltaT); 
     updatePosition(deltaT); 

    } 

Answer 1

Во-первых, создать добавить и раз функции в Vec3.

Если сфера s1 и сфера S2, с позиции р (vec3), скорость у (vec3), радиус г (с плавающей точкой) и массой т (флоат) сталкивается, вы можете сделать:

// from s1 to s2 
vec3 pDiff = s2.p.subtract(s1.p); 
// collision detection 
if (pDiff.getLength() >= s1.r + s2.r) { 
    return; 
} 
// find direction from s1 to s2 
vec3 dir = pDiff.normalize(); 
vec3 vDiff = s2.v.subtract(s1.v); 
float fellingSpeed = vDiff.dot(dir); 
// don't collide in this case 
if (fellingSpeed >= 0) { 
    return; 
} 
// perfect spheric collision 
float speed1 = (2 * s2.m * fellingSpeed)/(s1.m + s2.m); 
float speed2 = (fellingSpeed * (s2.m - s1.m))/(s1.m + s2.m); 
s1.v = s1.v.add(dir.times(speed1)); 
s2.v = s2.v.add(dir.times(speed2 - fellingSpeed)); 

Надеюсь, это поможет.