void getInfo2 (btConstraintInfo2* info) {
			btVector3 relA = m_rbA.getCenterOfMassTransform().getBasis() * getPivotInA();
			btVector3 relB = m_rbB.getCenterOfMassTransform().getBasis() * getPivotInB();
			btVector3 posA = m_rbA.getCenterOfMassTransform().getOrigin() + relA;
			btVector3 posB = m_rbB.getCenterOfMassTransform().getOrigin() + relB;
			btVector3 del = posB - posA;
			btScalar currDist = btSqrt(del.dot(del));
			btVector3 ortho = del / currDist;
			info->m_J1linearAxis[0] = ortho[0];
			info->m_J1linearAxis[1] = ortho[1];
			info->m_J1linearAxis[2] = ortho[2];
			btVector3 p, q;
			p = relA.cross(ortho);
			q = relB.cross(ortho);
			info->m_J1angularAxis[0] = p[0];
			info->m_J1angularAxis[1] = p[1];
			info->m_J1angularAxis[2] = p[2];
			info->m_J2angularAxis[0] = -q[0];
			info->m_J2angularAxis[1] = -q[1];
			info->m_J2angularAxis[2] = -q[2];
			btScalar rhs = (currDist - m_dist) * info->fps * info->erp;
			info->m_constraintError[0] = rhs;
			info->cfm[0] = btScalar(0.f);
			info->m_lowerLimit[0] = -SIMD_INFINITY;
			info->m_upperLimit[0] = SIMD_INFINITY;
		}
void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& body0_trans, const btTransform& body1_trans)
{
	btAssert(!m_useSolveConstraintObsolete);

	 //retrieve matrices

	// anchor points in global coordinates with respect to body PORs.
   
    // set jacobian
    info->m_J1linearAxis[0] = 1;
	info->m_J1linearAxis[info->rowskip+1] = 1;
	info->m_J1linearAxis[2*info->rowskip+2] = 1;

	btVector3 a1 = body0_trans.getBasis()*getPivotInA();
	{
		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
		btVector3 a1neg = -a1;
		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    
	info->m_J2linearAxis[0] = -1;
    info->m_J2linearAxis[info->rowskip+1] = -1;
    info->m_J2linearAxis[2*info->rowskip+2] = -1;
	
	btVector3 a2 = body1_trans.getBasis()*getPivotInB();
   
	{
	//	btVector3 a2n = -a2;
		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
    


    // set right hand side
	btScalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
    btScalar k = info->fps * currERP;
    int j;
	for (j=0; j<3; j++)
    {
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
		//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    }
	if(m_flags & BT_P2P_FLAGS_CFM)
	{
		for (j=0; j<3; j++)
		{
			info->cfm[j*info->rowskip] = m_cfm;
		}
	}

	btScalar impulseClamp = m_setting.m_impulseClamp;//
	for (j=0; j<3; j++)
    {
		if (m_setting.m_impulseClamp > 0)
		{
			info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
			info->m_upperLimit[j*info->rowskip] = impulseClamp;
		}
	}
	info->m_damping = m_setting.m_damping;
	
}
void btPoint2PointConstraint::getInfo2 (btConstraintInfo2* info)
{
    btAssert(!m_useSolveConstraintObsolete);

     //retrieve matrices
    btTransform body0_trans;
    body0_trans = m_rbA.getCenterOfMassTransform();
    btTransform body1_trans;
    body1_trans = m_rbB.getCenterOfMassTransform();

    // anchor points in global coordinates with respect to body PORs.

    // set jacobian
    info->m_J1linearAxis[0] = 1;
    info->m_J1linearAxis[info->rowskip+1] = 1;
    info->m_J1linearAxis[2*info->rowskip+2] = 1;

    btVector3 a1 = body0_trans.getBasis()*getPivotInA();
    {
        btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
        btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
        btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
        btVector3 a1neg = -a1;
        a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
    }

    /*info->m_J2linearAxis[0] = -1;
    info->m_J2linearAxis[s+1] = -1;
    info->m_J2linearAxis[2*s+2] = -1;
    */

    btVector3 a2 = body1_trans.getBasis()*getPivotInB();

    {
        btVector3 a2n = -a2;
        btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
        btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
        btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
        a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
    }



    // set right hand side
    btScalar k = info->fps * info->erp;
    int j;

    for (j=0; j<3; j++)
    {
        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] -                     a1[j] - body0_trans.getOrigin()[j]);
        //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
    }

    btScalar impulseClamp = m_setting.m_impulseClamp;//
    for (j=0; j<3; j++)
    {
        if (m_setting.m_impulseClamp > 0)
        {
            info->m_lowerLimit[j*info->rowskip] = -impulseClamp;
            info->m_upperLimit[j*info->rowskip] = impulseClamp;
        }
    }

}