153 lines
4.1 KiB
C++
153 lines
4.1 KiB
C++
/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
*
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
*
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
*
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
*
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
#include "dgConnectorConstraint.h"
|
|
#include "dgBody.h"
|
|
#include "hpl1/engine/libraries/newton/core/dg.h"
|
|
|
|
|
|
/*
|
|
class dgConnectorConstraintArray: public dgPoolContainer<dgConnectorConstraint>
|
|
{
|
|
public:
|
|
|
|
static dgConnectorConstraintArray& GetPool()
|
|
{
|
|
static dgConnectorConstraintArray pool;
|
|
return pool;
|
|
}
|
|
};
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// Construction/Destruction
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
dgConnectorConstraint::dgConnectorConstraint()
|
|
{
|
|
|
|
}
|
|
|
|
dgConnectorConstraint::~dgConnectorConstraint()
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
dgConnectorConstraint* dgConnectorConstraint::Create()
|
|
{
|
|
dgConnectorConstraint* constraint;
|
|
|
|
constraint = dgConnectorConstraintArray::GetPool().GetElement();
|
|
|
|
constraint->Init();
|
|
constraint->m_maxDOF = 6;
|
|
constraint->m_constId = dgConectorConstraintId;
|
|
return constraint;
|
|
}
|
|
|
|
void dgConnectorConstraint::Remove()
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
}
|
|
|
|
|
|
void dgConnectorConstraint::Setup ()
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
|
|
dgVector p0 (m_dynamic0->GetBody()->GetMatrix().posit);
|
|
dgVector p1 (p0);
|
|
if (m_dynamic1) {
|
|
p1 = m_dynamic1->GetBody()->GetMatrix().posit;
|
|
}
|
|
|
|
dgVector pivet (p1 + p0);
|
|
pivet = pivet.Scale (0.5f);
|
|
dgBallConstraint::SetPivotPoint (pivet);
|
|
|
|
}
|
|
|
|
|
|
dgUnsigned32 dgConnectorConstraint::JacobianDerivative (dgContraintDescritor& params)
|
|
{
|
|
NEWTON_ASSERT (0);
|
|
return 0;
|
|
|
|
// dgFloat32 k;
|
|
dgBody* body0;
|
|
dgBallConstraint::JacobianDerivative (params);
|
|
|
|
body0 = m_dynamic0->GetBody();
|
|
const dgMatrix& mat0 = body0->GetMatrix();
|
|
dgJacobian* jacobian01 = ¶ms.m_jacobian_01[3];
|
|
|
|
jacobian01[0].derivative[0] = 0.0f;
|
|
jacobian01[0].derivative[1] = 0.0f;
|
|
jacobian01[0].derivative[2] = 0.0f;
|
|
jacobian01[0].derivative[3] = mat0.front.m_x;
|
|
jacobian01[0].derivative[4] = mat0.front.m_y;
|
|
jacobian01[0].derivative[5] = mat0.front.m_z;
|
|
|
|
jacobian01[1].derivative[0] = 0.0f;
|
|
jacobian01[1].derivative[1] = 0.0f;
|
|
jacobian01[1].derivative[2] = 0.0f;
|
|
jacobian01[1].derivative[3] = mat0.up.m_x;
|
|
jacobian01[1].derivative[4] = mat0.up.m_y;
|
|
jacobian01[1].derivative[5] = mat0.up.m_z;
|
|
|
|
jacobian01[2].derivative[0] = 0.0f;
|
|
jacobian01[2].derivative[1] = 0.0f;
|
|
jacobian01[2].derivative[2] = 0.0f;
|
|
jacobian01[2].derivative[3] = mat0.right.m_x;
|
|
jacobian01[2].derivative[4] = mat0.right.m_y;
|
|
jacobian01[2].derivative[5] = mat0.right.m_z;
|
|
|
|
if (m_dynamic1) {
|
|
dgJacobian* jacobian10 = ¶ms.m_jacobian_10[3];
|
|
jacobian10[0].derivative[0] = 0.0f;
|
|
jacobian10[0].derivative[1] = 0.0f;
|
|
jacobian10[0].derivative[2] = 0.0f;
|
|
jacobian10[0].derivative[3] = -mat0.front.m_x;
|
|
jacobian10[0].derivative[4] = -mat0.front.m_y;
|
|
jacobian10[0].derivative[5] = -mat0.front.m_z;
|
|
|
|
jacobian10[1].derivative[0] = 0.0f;
|
|
jacobian10[1].derivative[1] = 0.0f;
|
|
jacobian10[1].derivative[2] = 0.0f;
|
|
jacobian10[1].derivative[3] = -mat0.up.m_x;
|
|
jacobian10[1].derivative[4] = -mat0.up.m_y;
|
|
jacobian10[1].derivative[5] = -mat0.up.m_z;
|
|
|
|
jacobian10[2].derivative[0] = 0.0f;
|
|
jacobian10[2].derivative[1] = 0.0f;
|
|
jacobian10[2].derivative[2] = 0.0f;
|
|
jacobian10[2].derivative[3] = -mat0.right.m_x;
|
|
jacobian10[2].derivative[4] = -mat0.right.m_y;
|
|
jacobian10[2].derivative[5] = -mat0.right.m_z;
|
|
}
|
|
|
|
// k = 0.75f * params.m_invTimestep;
|
|
// dgVector angle (front0 * front1);
|
|
}
|
|
*/
|