Facing two objects...

[Fax3D]

Hi All,

i'm trying to do a very complex thing and i need help to solve a issue. Imagine you have this scenario:

A Wall, built using a 5X5 cube matrix, so the entire Wall is a compound body with 25 different cubes entity.
A compound body, built with 4 different cubes placed to create a "L" shape.

So i grab the "L" object, i shot this toward the wall and i want to make it join perfectly to the surface of the
wall whatever is the angle of impact, obviously making facing the nearest face of the L object.

My problem is how to build the rotation matrix to make the objects facing parallel each other, because all the rest
works perfectly (i dynamically add the L object to the wall's compound body when there's a collision as well)

I'm quite sure the answer is in the BEPUphysics.Contact i receive from EventHandlerInitialCollisionDetected
callback, but i don't know how to use all the information stored!

Can anyone helps me to solve this problem?

Thanks a lot!

Fax3D

[Norbo]

Assuming I'm understanding the situation right (as shown in the picture below), here's a relatively simple way to approach it.

First, get the rotation matrix from the incoming L. Go through each of the three axes (matrix.Up, matrix.Right, Matrix.Forward) and dot them against the contact point's normal (represented from here on out by a vector called N). Keep the axis (represented from here on out by a vector called A) associated with the largest absolute value dot product; this axis represents the axis that is the closest to being aligned with the normal. Once you've identified A, check the dot product's sign- if it is negative, negate A. This ensures that N and A are pointing the same way.

Now we need to find a matrix that transforms A to be the same as N. One way to do this is using a rotation matrix created from an axis and angle. We can get this information by taking the cross product of A and N (the result of which will be represented by C). Get the length of C and compute the angle by using Math.Sin(length). Normalize C by dividing by the length. Using one of XNA's built in functions, Matrix.CreateFromAxisAngle, compute a rotation matrix (from here on out referred to as R1).

Since R1 represents only the transformation to align the face flatly, we still need to find a transformation that will screw the shape into a squarely aligned position. The process to do this is very similar to the above. We first grab a copy of the entity's rotation matrix and multiply it by R1. Now, one of this new matrix's axes is aligned along the surface normal. Of these other two axes, find the one which most closely aligns with the up axis of the wall's rotation matrix. This is done by using the dot product on the other two axes to find the one with a larger absolute value. If the largest absolute value dot product is negative, negate the vector associated with it and its dot product. Now we want to rotate the system to align, but we already know the axis (the surface normal) and we already have a dot product that can tell us the angle (by using Math.Cos(dot product result)). Create another matrix R2 using Matrix.CreateFromAxisAngle.

Multiplying the original rotation matrix by R1 and R2 should give a new fully aligned orientation.

Another option is to modify the orthonormal basis defined by the approaching object's rotation matrix so that it has the closest possible orientation to the wall. The 'difference rotation' R of these two rotation matrices can be found quickly by using R = N1 * tranpose(N2) where N2 is the approaching object's modified rotation matrix and N1 is the wall's rotation matrix.

I was in a bit of a rush writing this up, so I hope it isn't too error-filled. I'll be back later to answer questions and correct anything I messed up.

[Fax3D]

Norbo, i've no words to explain how you was useful for me! Thanks very very a lot this is exactly what i need to do! Now i'm going to write the code... i'll make you know if it's all right and if your great solution works.

Thanks

Fax3D

[Fax3D]

Hi Norbo,

i writed the code, the things seem to be in the right direction but it's still not works very well. Take a look at this two screens:

This is without any correction, just a cube shooted toward the wall with its own collision angle:
http://img9.imageshack.us/my.php?image=beforer.jpg

This applying the correction you suggested:
http://img9.imageshack.us/my.php?image=afterabe.jpg

It seems to be too much rotation... i show you the way i calculate R1 (notice i made only the first part of yours solution, the one who try to find a rotation matrix R1) :

Code: Select all

    foreach (Entity entity in m_PhysicsBody.subBodies)
    { 
        entity.addEventHook(new BEPUphysics.Events.EventHandlerInitialCollisionDetected(InitialCollisionDetected));
    }

    public void InitialCollisionDetected(Entity entity, BEPUphysics.Controller controller) 
    {
                EntityObjectDefinition _tagA = (EntityObjectDefinition)controller.colliderA.tag;
                EntityObjectDefinition _tagB = (EntityObjectDefinition)controller.colliderB.tag;

                if (_tagA.Jointed == 1)
                        return;

                Vector3 _up = controller.colliderA.orientationMatrix.Up;
                Vector3 _right = controller.colliderA.orientationMatrix.Right;
                Vector3 _forward = controller.colliderA.orientationMatrix.Forward;

                Vector3 N = controller.contacts[0].normal;

                float _dotUP = Math.Abs(Vector3.Dot(_up, N));
                float _dotRight = Math.Abs(Vector3.Dot(_right, N));
                float _dotForward = Math.Abs(Vector3.Dot(_forward, N));

                Vector3 A = new Vector3(0, 0, 0);

                if (_dotUP >= _dotRight && _dotUP >= _dotForward)
                    A = _up;

                if (_dotRight >= _dotUP && _dotRight >= _dotForward)
                    A = _right;

                if (_dotForward >= _dotRight && _dotForward >= _dotUP)
                    A = _forward;

                if (Vector3.Dot(A, N) < 0)
                    A = Vector3.Negate(A);

                Vector3 C = Vector3.Cross(A, N);
                float _lenght =  C.Length();
                double _angle = Math.Sin(_lenght);

                C = Vector3.Divide(C, _lenght);
                C = Vector3.Normalize(C);

                Matrix R1 = Matrix.CreateFromAxisAngle(C, Convert.ToSingle(_angle));

                _tagA.R1 = R1;
                _tagA.Jointed = 1;
                controller.colliderA.tag = _tagA;
      }

       public void Update()
       {
           ....
           m_PhysicsBody.orientationMatrix = _tag.R1;
           ....
       }

Probably i made any error in the code...i also tried to divide by 2 the angle, to not normalize... but the result it's quite the same.

Thanks!

Fax3D

UPDATE: It seems to work right without doing "C = Vector3.Divide(C, _lenght);"...

[Fax3D]

Ok, now the first part works very well, i'm able to align the face flatly to the wall. The right code should be this:

Code: Select all

                Vector3 _up = controller.colliderA.orientationMatrix.Up;
                Vector3 _right = controller.colliderA.orientationMatrix.Right;
                Vector3 _forward = controller.colliderA.orientationMatrix.Forward;

                Vector3 N = controller.contacts[0].normal;

                float _dotUP = Math.Abs(Vector3.Dot(_up, N));
                float _dotRight = Math.Abs(Vector3.Dot(_right, N));
                float _dotForward = Math.Abs(Vector3.Dot(_forward, N));

                Vector3 A = new Vector3(0, 0, 0);

                if (_dotUP >= _dotRight && _dotUP >= _dotForward)
                    A = _up;

                if (_dotRight >= _dotUP && _dotRight >= _dotForward)
                    A = _right;

                if (_dotForward >= _dotRight && _dotForward >= _dotUP)
                    A = _forward;

                if (Vector3.Dot(A, N) < 0)
                    A = Vector3.Negate(A);

                Vector3 C = Vector3.Cross(A, N);
                float _lenght =  C.Length();
                double _angle = Math.Sin(_lenght);

                //C = Vector3.Divide(C, _lenght);

                Matrix R1 = Matrix.CreateFromAxisAngle(Vector3.Normalize(C), Convert.ToSingle(_angle));

But currently i'm not able to do right the second part, even here seems to be too much angle. This is the second part wich starts just after the last line of code above:

Code: Select all

                Matrix _rot = controller.colliderA.orientationMatrix * R1;

                _up = _rot.Up;
                _right = _rot.Right;
                _forward = _rot.Forward;

                _dotUP = Math.Abs(Vector3.Dot(_up, C));
                _dotRight = Math.Abs(Vector3.Dot(_right, C));
                _dotForward = Math.Abs(Vector3.Dot(_forward, C));

                A = new Vector3(0, 0, 0);

                if (_dotUP >= _dotRight && _dotUP >= _dotForward)
                    A = _up;

                if (_dotRight >= _dotUP && _dotRight >= _dotForward)
                    A = _right;

                if (_dotForward >= _dotRight && _dotForward >= _dotUP)
                    A = _forward;

                if (Vector3.Dot(A, C) < 0)
                    A = Vector3.Negate(A);

                Vector3 D = Vector3.Cross(A, C);
                _lenght = D.Length();
                _angle = Math.Sin(_lenght);

                Matrix R2 = Matrix.CreateFromAxisAngle(Vector3.Normalize(C), Convert.ToSingle(_angle));

                Matrix _final = controller.colliderA.orientationMatrix * R1 * R2;

Where could be the error?

Thanks

Fax3D

[Norbo]

Instead of comparing the axes to C, compare them against one of the axes of the wall's rotation. Note that one of these axes will line up with the surface normal while the other two will be on the plane perpendicular to the surface normal. One of these other two axes should be the axis compared against. This is because we want to find the rotation required to align one of the approaching box's own two normal-perpendicular axes with that of the wall.

So basically, pick an axis that isn't the surface normal from the wall's orientation matrix axes and use it instead of C. Assuming some other information wasn't stored allowing you to find this axis, you could compare the wall's orientation matrix axes against the surface normal. The first axis which has a dot product close to 0 (within a tiny tolerance to allow for floating point error) can be used for this purpose.

[Fax3D]

Ok, now it seems to work well but there's an issue: when i aplly the final rotation matrix to make the L and the wall facing perfectly, by doing (entity).orientationMatrix * R1 * R2, this is applied at the geometric center (or at mass center i don't know) of the L object!!! But to have a right behaviour this rotation must be applied at the first contact point between L and Wall i believe... in fact the final result is not correct this way
So, what's the best way to resolve this issue?

Thanks a lot in advance!

Fax3D

[Norbo]

The rotation matrix you've computed won't change; you'll just need to translate the shape some amount. Find the vector from the contact point position to the initial center of the shape. Transform this vector by the rotation matrix you compute. The post-rotation position of the object should be the contact's position added to the transformed offset.

Further, if you wanted the boxes to align (sort of "Tetris" style), then you need to apply one more translation. Since you've got a regular grid and what amount to "X" and "Y" axes defined by the wall's orientation matrix, you can compare the position of the colliding subentity to the grid. Find a position that 'fits' into the grid correctly and translate the shape. Using the Vector3.Dot on the center position of the subentity and an axis of the wall's orientation matrix will give you the position on that axis of the entity.

[Fax3D]

Yes great, now all works good!

Thanks!

Fax3D