How to make an object face another

[arkdemon]

Hello everyone. I have a question to ask: How do we make an object face another one (with torque)?
Thank you for your answers

[JoeJ]

Some pseudo code:

Code: Select all

matrix = BodyGetMatrix (body);
float mass, Ixx, Iyzz Izz = BodyGetMassMatrix (...)

vector currentDir = matrix.Rotate (0,0,1); // say local z is look direction of body
vector targetDir (1,0,0); // it should look at world x direction
float axis, vector angle = MakeAxisAndAngle (currentDir, targetDir);

vector targetAngVel = angle * angle / timestep; // make angular velocity from axis and angle (optional: scale by factor between 0.0 and 1.0 - see below)
vector curAngVel = BodyGetOmega();
targetAngVel -= curAngVel;

// here you could try NewtonBodySetOmega(targetAngVel), but using torque is better...

vector torque = targetAngVel / timestep; // make torque from angular velocity...
torque = matrix.Unrotate (torque); // go to local space to apply inertia
torque[0] *= Ixx;
torque[1] *= Iyy;
torque[2] *= Izz;
torque = matrix.Rotate (torque); // back to world space

torque *= 0.3; // something between 0.0 and 1.0; recommended <0.5, 1.0 would do full rotation in one step, but will generate overspin



[arkdemon]

Hi, thank you for your answer. What is the function MakeAxisAndAngle?
I have:

Code: Select all

static void angleBetween2(glm::vec3 v1,glm::vec3 v2, glm::vec3 &axis, float &angle) {
    // turn vectors into unit vectors
    glm::vec3 n1 = glm::normalize(v1);
    glm::vec3 n2 = glm::normalize(v2);
    angle = glm::acos(glm::dot(n1,n2));
    // if no noticable rotation is available return zero rotation
    // this way we avoid Cross product artifacts
    if( glm::abs(angle) < 0.0001f)
        axis = glm::vec3(0.f,0.f,1.f);
    else
        axis = glm::cross(n1, n2);
}

but it does not work.
I took it from here: http://www.euclideanspace.com/maths/alg ... /index.htm

and here is my code:

Code: Select all

 float m[16];
            NewtonBodyGetMatrix (body, m);
            dgMatrix matrix(m);

            float mass, Ixx, Iyy, Izz;
            NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);

            float angle;
            glm::vec3 axis;
            angleBetween2(s.pos,e->mset().pos,axis,angle);//e is the other body and "s" and "mset()" is the struct containing the position, torque, force, ...

            glm::vec3 targetAngVel = axis * angle; // make angular velocity from axis and angle
            float v[3];
            NewtonBodyGetOmega(body, v);
            glm::vec3 curAngVel = glm::make_vec3(v);
            targetAngVel -= curAngVel;

            dgVector torque = dgVector(glm::value_ptr(targetAngVel*angle)); // make torque from angular velocity...
            torque = matrix.UnrotateVector(torque); // go to local space to apply inertia
            torque[0] *= Ixx;
            torque[1] *= Iyy;
            torque[2] *= Izz;
           
            torque = matrix.RotateVector(torque); // back to world space


Thank you for your help

[JoeJ]

else
{
axis = glm::cross(n1, n2);
axis.Normalize();
}

...the axis is expected to have unit length.
Does it work then? It should, even it's not the fastest way to do it.

[arkdemon]

Hi. Thank you for your quick answer.
It still does not work. Is there a problem with my source?

[JoeJ]

dgVector torque = dgVector(glm::value_ptr(targetAngVel*angle)); // make torque from angular velocity...

should be

dgVector torque = dgVector(glm::value_ptr(targetAngVel / timestep)); // make torque from angular velocity...

...and now?

[arkdemon]

Hi, no it still does not work.

[JoeJ]

It really should, i've copied the code and it works for me.
I use other function and class names, but you only need to add 'Newton' prefix, sMat4 is like dgMatrix and sVec3 is usual vector.
The only difference is the axis and angle stuff, where i first create a quaternieon and convert to axis / angle.

the torque is applied in Force Torque callback:

Code: Select all

void ForceAndTorqueCallback (const NewtonBody* body, dFloat timestep, int threadIndex)
{
   dFloat Ixx;
   dFloat Iyy;
   dFloat Izz;
   dFloat mass;

   BodyData *data = (BodyData*) BodyGetUserData (body);

   sVec3 force (0.0f, mass * propsPhysics::f[propsPhysics::gravity], 0.0f);

   
   force += data->force;
   NewtonBodyAddForce (body, &force[0]);
   sVec3 torque = data->torque;
   NewtonBodyAddTorque (body, &torque[0]);
   
data->ClearForces(); // set torque and force to zero

}


And here the relevant Quaternion stuff:

Code: Select all


//quaternion is four floats (x,y,z,w), first three is xyz vector and 4th is scalar w part

__forceinline void FromVecToVecNonUniformLength (const sVec3 &dir0, const sVec3 &dir1)
   {
      *((sVec3*)this) = dir0.Cross(dir1); // cast to vector part
      (*this)[3] = 1.0 + dir0.Dot(dir1); // cast to scalar part
      Normalize(); // like a vector normalization, but with all four values, not just three
   }

__forceinline void ToAxisAndAngle (sVec3 &axis, sScalar &angle) //const
   {
      sScalar length2 = ((sVec3*)this)->SqL(); // squared length of vector part = this.Dot(this)
      if ( length2 > FP_EPSILON2)
      {
         angle = 2.0 * acos(w());
         float invlen = 1.0f / sqrt(length2);
         axis = *((sVec3*)this) * invlen;
      }
      else
      {
         angle = 0;
         axis = sVec3 (1.0, 0.0, 0.0);
      }
}


Here the rest of the code...

Code: Select all

Body *bLook, *bAt;

   void CreateLookAtTest () // scene setup
   {
      sMat4 m; m.Identity(); m.Reset4thColumn();

      Shape *shape = world->CreateBoxShape (sVec3(2.0, 0.5, 0.5));
      m[3] = sVec3 (0, 1.0, 0);
      bLook = world->CreateRigidBody (2.0, m, shape);
      world->ReleaseShape (shape);

      shape = world->CreateBoxShape (sVec3(0.5, 0.5, 0.5));
      m[3] = sVec3 (0, 1.0, 3.0);
      bAt = world->CreateRigidBody (2.0, m, shape);
      world->ReleaseShape (shape);

   }

   void StepLookAtTest (float timestep) // called before newton update
   {
      sMat4 m;
      BodyGetMatrix (bLook, m);
       
      float mass, Ixx, Iyy, Izz;
        BodyGetMassMatrix (bLook, mass, Ixx, Iyy, Izz);

      sMat4 m2;
      BodyGetMatrix (bAt, m2);
      sVec3 &targetPos = m2[3];      

      sVec3 axis; float angle;
      sQuat rot; rot.FromVecToVecNonUniformLength (m[0], targetPos - m[3]);
      rot.ToAxisAndAngle (axis, angle);

      sVec3 targetAngVel = axis * angle / timestep * 0.3; // make angular velocity from axis and angle
      sVec3 curAngVel;
      BodyGetAngVel (bLook, curAngVel); // == NewtonBodyGetOmega (bLook, &curAngVel[0]);
      targetAngVel -= curAngVel;

      sVec3 torque = targetAngVel / timestep; // make torque from angular velocity...
      torque = m.Unrotate(torque); // go to local space to apply inertia
      torque[0] *= Ixx;
      torque[1] *= Iyy;
      torque[2] *= Izz;
      torque = m.Rotate(torque); // back to world space

      BodyData *data = (BodyData*) BodyGetUserData(bLook);
      data->torque += torque;
   }


I'll check your axis angle stuff becaus mine may be difficult to read...

[JoeJ]

... works too:

Code: Select all

static void angleBetween2 (sVec3 n1, sVec3 n2, sVec3 &axis, float &angle)
   {
        // turn vectors into unit vectors
        n1.Normalize();
      n2.Normalize();
        angle = acos(n1.Dot(n2));
        // if no noticable rotation is available return zero rotation
        // this way we avoid Cross product artifacts
        if(fabs(angle) < 0.0001f)
            axis = sVec3(0.f,0.f,1.f);
        else
            axis = sVec3(n1.Cross(n2)).Unit();
    }


and changing:

Code: Select all

sVec3 axis; float angle;
//sQuat rot; rot.FromVecToVecNonUniformLength (m[0], targetPos - m[3]);
//rot.ToAxisAndAngle (axis, angle);
angleBetween2 (m[0], targetPos - m[3], axis, angle);


[arkdemon]

Hello. Sorry it still does not work. So here is my code:
I am trying to make an object fire some bullets continously so I do it like this:
to shoot

Code: Select all

    Body *shoot(){//shoot a bullet from the front of the object
        dgMatrix m(glm::value_ptr(s.model));
        dgVector v = m.m_front;
        glm::vec3 pp = -glm::make_vec3(&v[0]);
        float mass = msphere*mass_metal+1.f;
        float life = 1.f;//mass*l_mult;
        MovSet mset(s.pos+pp, mass, life);
        Body *b = new Body(mset, def_mat, NewtonCreateSphere(world, 10.f*SCALE,0, NULL));
        NewtonBodySetVelocity(b->body, glm::value_ptr(pp*10.f));
        //NewtonBodySetContinuousCollisionMode(b->body, 1);
        return b;
    }

to fire continously (with the direction change and all)

Code: Select all

    Body *update(float ts){ //return NULL if the body does not create any body, otherwise yes and it pushes a body in a vector for collision
        if(is_fire){
            if(e->mset().life <= 0.f){ //either body is "dead" or body is invincible like flat terrain
                e = NULL;
                is_fire = false;
                return NULL;
            }
            float m[16];
            NewtonBodyGetMatrix (body, m);
            dgMatrix matrix(m);
            glm::mat4 m2 = glm::make_mat4(m);

            float mass, Ixx, Iyy, Izz;
            NewtonBodyGetMassMatrix(body, &mass, &Ixx, &Iyy, &Izz);

            float angle;
            glm::vec3 axis;
            /*glm::quat rot;
            FromVecToVecNonUniformLength (rot, glm::vec3(m2[0]), e->mset().pos - glm::vec3(m2[3]));
            ToAxisAndAngle (rot, axis, angle);*/
            angleBetween2 (glm::vec3(m[0]), glm::vec3(e->mset().model[3]) - glm::vec3(m[3]), axis, angle);

            glm::vec3 targetAngVel = axis * angle; // make angular velocity from axis and angle
            float v[3];
            NewtonBodyGetOmega(body, v);
            targetAngVel -= glm::make_vec3(v);

            dgVector torque = dgVector(glm::value_ptr(targetAngVel)); // make torque from angular velocity...
            torque = matrix.UnrotateVector(torque); // go to local space to apply inertia
            torque[0] *= Ixx;
            torque[1] *= Iyy;
            torque[2] *= Izz;

            torque = matrix.RotateVector(torque); // back to world space

            s.torque += glm::make_vec3(&torque[0]);
            return shoot();
        }
        return NULL;
    }

In the physics class (the Body class), my callbacks are those:

Code: Select all

#define GRAVITY 1.f
static glm::vec3 GRAVITY_POINT = glm::vec3(0.f);

static void ApplyTransform(const NewtonBody* body, const dFloat* matrix, int threadId){
    dgMatrix m(matrix);
    MovSet *mset = (MovSet*) NewtonBodyGetUserData(body);
    glm::vec3 p = glm::make_vec3(&m.m_posit[0]);
    mset->pos = p;
    mset->model = glm::make_mat4(matrix);
}

static void ApplyForceAndTorque(const NewtonBody* body, dFloat timestep, int threadId){

    MovSet *mset = (MovSet*) NewtonBodyGetUserData(body);

    glm::vec3 gravity = (GRAVITY_POINT-mset->pos)*GRAVITY*mset->mass;
#ifdef Y_GRAVITY
    mset->force.y += gravity.y;//gravity;
#else
    mset->force += gravity;//gravity;
#endif

    NewtonBodyAddForce(body, glm::value_ptr(mset->force));
    NewtonBodyAddTorque(body, glm::value_ptr(mset->torque));

    mset->force = glm::vec3(0.f);
    mset->torque = glm::vec3(0.f);
}


where MovSet is this:

Code: Select all

struct MovSet{
    glm::vec3 pos;
    glm::vec3 force, torque;
    glm::mat4 model;
    float mass;
    float life;
    float rlife;

    unsigned int id, g;

    MovSet(glm::vec3 p, float m, float l, float rl = 0.f)
        : pos(p), mass(m), id(0), g(0), force(0.f), torque(0.f), model(1.f), life(l), rlife(rl){
        force = glm::vec3(0.f);
        torque = glm::vec3(0.f);
        model = glm::mat4(1.f);
    }

    void use(unsigned int _g, unsigned int _id){
        g = _g;
        id = _id;
    }

    void damage(float l){
        life -= l;
    }
};

and thank you a lot for the time you spent trying to help me. I am very grateful.

EDIT:
anglebetween2 is:

Code: Select all

static void angleBetween2(glm::vec3 v1,glm::vec3 v2, glm::vec3 &axis, float &angle) {
    // turn vectors into unit vectors
    glm::vec3 n1 = glm::normalize(v1);
    glm::vec3 n2 = glm::normalize(v2);
    angle = glm::acos(glm::dot(n1,n2));
    // if no noticable rotation is available return zero rotation
    // this way we avoid Cross product artifacts
    /*if( glm::abs(angle) < 0.0001f)
        axis = glm::vec3(0.f,0.f,1.f);
    else*/
    axis = glm::normalize(glm::cross(n1, n2));
}

and for the quats it is:

Code: Select all

inline void FromVecToVecNonUniformLength (glm::quat q, const glm::vec3 &dir0, const glm::vec3 &dir1)
{
    glm::vec3 v = glm::cross(dir0,dir1); // cast to vector part
    q.x = v.x;
    q.y = v.y;
    q.z = v.z;
    q.w = 1.f + glm::dot(dir0,dir1); // cast to scalar part
    q = glm::normalize(q);
}

inline void ToAxisAndAngle (glm::quat q, glm::vec3 &axis, float &angle) //const
   {
      float length2 = glm::length(glm::vec3(q.x, q.y, q.z)); // squared length of vector part = this.Dot(this)
      if ( length2 > 8.854e-12f)
      {
         angle = 2.f * glm::acos(q.w);
         float invlen = 1.f / glm::sqrt(length2);
         axis = glm::vec3(q.x, q.y, q.z) * invlen;
      }
      else
      {
         angle = 0.f;
         axis = glm::vec3(1.f, 0.f, 0.f);
      }
}

(but it does not seem to work)

[JoeJ]

Your code does not look wrong, but maybe you want something completely different than i think.

What happens with my coe is this:
Generate 2 bodies.
When i pick the bAt body and move it around the scene, the bLook body rotates (very slowly) to 'keep looking' at the bAt body.
If i want a faster rotation, i'll need to change... oops, there's another bug - change:
sVec3 targetAngVel = axis * angle / timestep * 0.9; // make angular velocity from axis and angle // FORGOT TO DIVIDE BY TIMESTEP HERE
The 0.9 makes it fast, 0.05 is slow.
So this is tool to control the orientation of objects, f. ex. aligning a carried object to player controller.

But you say you want to shoot stuff?
I can't follow your code - can you describe what bahaviour you want?
I don't understand why orientation is important for this purpose.
Usually the question her is: What launch angle do i need to fire an object so it will hit a target (projectile trajectory).

[arkdemon]

Hi, thank you for your answer.
So this is what I want to do:
I click on a body then I click on an other one. The first body will shoot on the other clicked body until it "dies". So the first body needs to orientate itself to shoot properly or it will shoot endlessly nowhere.

the shoot() function is the function which makes the body shoot a single bullet
the fire() function is the function that initializes the continuous shooting (ie, set "e" the other body and is_fired = true)
the update() function is the continuous shooting function. It returns a Body (the bullet) so that I can push it in my list of Bodies and render it. It is in this update() function that I rotate my body to the other body.

I hope it is clearer.

[arkdemon]

It works!!!!!
Thank you so much!
I understood what was the problem: it was the glm::mat4 matrix (s.model). I updated it only with ApplyTransform: now I removed the s.model and I call each turn this:

Code: Select all

            float m[16];
            NewtonBodyGetMatrix (body, m);

and it works now.
Thank you a LOT for your help.

PS: I do this

Code: Select all

glm::vec3 targetAngVel = -axis * angle;

because when there is not the "-" it goes on the other side

[JoeJ]

fine, it sould also work to use glm stuff directly with newton, like

glm::mat4 matrix;
NewtonBodyGetMatrix (body, (float*)&matrix);
or
NewtonBodyGetMatrix (body, matrix[0][0]);

glm::vec3 angVel;
NewtonBodyGetOmega (body, (float*)&angVel);
or
NewtonBodyGetOmega (body, angVel[0]);

etc...
See what works for you, you can avoid a lot of useless memory copying and typing

[arkdemon]

Ok