Fixed Joint

[pHySiQuE]

Here is the source code for my fixed joint. The child is entity[0], the parent is entity[1]:

Code: Select all

            fixedjointchildrelativeposition=Transform::Point(entity[0]->GetPosition(true),NULL,entity[1]);
            fixedjointchildrelativeaxes[0]=Transform::Vector(1,0,0,NULL,entity[0]);
            fixedjointchildrelativeaxes[1]=Transform::Vector(0,1,0,NULL,entity[0]);
            fixedjointchildrelativeaxes[2]=Transform::Vector(0,0,1,NULL,entity[0]);
            fixedjointparentrelativeaxes[0]=Transform::Vector(1,0,0,NULL,entity[1]);
            fixedjointparentrelativeaxes[1]=Transform::Vector(0,1,0,NULL,entity[1]);
            fixedjointparentrelativeaxes[2]=Transform::Vector(0,0,1,NULL,entity[1]);
            newtonjoint = NewtonConstraintCreateUserJoint(((NewtonDynamicsSimulation*)world->simulation)->newtonworld,6,NewtonUserBilateralCallBack,NULL,((NewtonDynamicsBody*)entity[0]->body)->body, ((NewtonDynamicsBody*)entity[1]->body)->body);
            NewtonJointSetUserData(newtonjoint, this);

Code: Select all

    //http://newtondynamics.com/forum/viewtopic.php?f=15&t=4056
    void NewtonDynamicsJoint::NewtonUserBilateralCallBack(const NewtonJoint* newtonjoint, dFloat timestep, int threadIndex)
    {       
      dFloat matrix0[16];
        dFloat matrix1[16];
        Mat4 mat0,mat1;
        Vec3 desiredposition;
        float dpf[3];
        Vec3 p0,p1;
        float pf0[3],pf1[3];
        float dist=5000;
        float stiffness=1;
        const NewtonBody* childbody = NewtonJointGetBody0(newtonjoint);
        const NewtonBody* parentbody = NewtonJointGetBody1(newtonjoint);
        Mat4 identity;
        dFloat axis[3];
        float trans[3];
        NewtonDynamicsJoint* newtondynamicsjoint = (NewtonDynamicsJoint*)NewtonJointGetUserData(newtonjoint);
       
        NewtonBodyGetMatrix(parentbody, matrix0);
        NewtonBodyGetMatrix(childbody, matrix1);
        mat0=Mat4(matrix0);
        mat1=Mat4(matrix1);
        float row[4];
        Vec3 mat0trans = mat0.GetTranslation();
        Vec3 mat1trans = mat1.GetTranslation();
       
        desiredposition = Transform::Point(newtondynamicsjoint->fixedjointchildrelativeposition,mat0,identity);
        dpf[0]=desiredposition.x; dpf[1]=desiredposition.y; dpf[2]=desiredposition.z;
       
      //Set the position - transform desired position from parent to world and add linear rows
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        axis[0]=1; axis[1]=0; axis[2]=0;
      NewtonUserJointAddLinearRow( newtonjoint, &matrix1[12], dpf, axis);
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        axis[0]=0; axis[1]=1; axis[2]=0;
      NewtonUserJointAddLinearRow( newtonjoint, &matrix1[12], dpf, axis);
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        axis[0]=0; axis[1]=0; axis[2]=1;
      NewtonUserJointAddLinearRow( newtonjoint, &matrix1[12], dpf, axis);       
       
      //Set rotation
      p0 = mat0trans + Transform::Vector(newtondynamicsjoint->fixedjointparentrelativeaxes[2],mat0,identity) * dist;
      p1 = mat1trans + Transform::Vector(newtondynamicsjoint->fixedjointchildrelativeaxes[2],mat1,identity) * dist;
      pf0[0]=p0.x; pf0[1]=p0.y; pf0[2]=p0.z;
        pf1[0]=p1.x; pf1[1]=p1.y; pf1[2]=p1.z;
      
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        trans[0]=mat0.j.x; trans[1]=mat0.j.y; trans[2]=mat0.j.z;
        NewtonUserJointAddLinearRow(newtonjoint, pf1, pf0, trans );
      
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        trans[0]=mat0.i.x; trans[1]=mat0.i.y; trans[2]=mat0.i.z;
      NewtonUserJointAddLinearRow(newtonjoint, pf1, pf0, trans );
      
      p0 = mat0trans + Transform::Vector(newtondynamicsjoint->fixedjointparentrelativeaxes[1],mat0,identity) * dist;
      p1 = mat1trans + Transform::Vector(newtondynamicsjoint->fixedjointchildrelativeaxes[1],mat1,identity) * dist;
      pf0[0]=p0.x; pf0[1]=p0.y; pf0[2]=p0.z;
        pf1[0]=p1.x; pf1[1]=p1.y; pf1[2]=p1.z;
       
      NewtonUserJointSetRowStiffness(newtonjoint,stiffness);
        trans[0]=mat0.k.x; trans[1]=mat0.k.y; trans[2]=mat0.k.z;
      NewtonUserJointAddLinearRow (newtonjoint, pf1, pf0, trans );
    }

[pHySiQuE]

Modification to NewtonUserBilateralCallBack(). It wasn't working right.