void IK_SetTransform(IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
IK_QSegment *qseg = (IK_QSegment*)seg;
MT_Vector3 mstart(start);
// convert from blender column major to moto row major
MT_Matrix3x3 mbasis(basis[0][0], basis[1][0], basis[2][0],
basis[0][1], basis[1][1], basis[2][1],
basis[0][2], basis[1][2], basis[2][2]);
MT_Matrix3x3 mrest(rest[0][0], rest[1][0], rest[2][0],
rest[0][1], rest[1][1], rest[2][1],
rest[0][2], rest[1][2], rest[2][2]);
MT_Scalar mlength(length);
if (qseg->Composite()) {
MT_Vector3 cstart(0, 0, 0);
MT_Matrix3x3 cbasis;
cbasis.setIdentity();
qseg->SetTransform(mstart, mrest, mbasis, 0.0);
qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
}
else
qseg->SetTransform(mstart, mrest, mbasis, mlength);
}
/* vectomat function obtained from constrain.c and modified to work with MOTO library */
static MT_Matrix3x3 vectomat(MT_Vector3 vec, short axis, short upflag, short threedimup)
{
MT_Matrix3x3 mat;
MT_Vector3 y(MT_Scalar(0.0f), MT_Scalar(1.0f), MT_Scalar(0.0f));
MT_Vector3 z(MT_Scalar(0.0f), MT_Scalar(0.0f), MT_Scalar(1.0f)); /* world Z axis is the global up axis */
MT_Vector3 proj;
MT_Vector3 right;
MT_Scalar mul;
int right_index;
/* Normalized Vec vector*/
vec = vec.safe_normalized_vec(z);
/* if 2D doesn't move the up vector */
if (!threedimup) {
vec.setValue(MT_Scalar(vec[0]), MT_Scalar(vec[1]), MT_Scalar(0.0f));
vec = (vec - z.dot(vec)*z).safe_normalized_vec(z);
}
if (axis > 2)
axis -= 3;
else
vec = -vec;
/* project the up vector onto the plane specified by vec */
/* first z onto vec... */
mul = z.dot(vec) / vec.dot(vec);
proj = vec * mul;
/* then onto the plane */
proj = z - proj;
/* proj specifies the transformation of the up axis */
proj = proj.safe_normalized_vec(y);
/* Normalized cross product of vec and proj specifies transformation of the right axis */
right = proj.cross(vec);
right.normalize();
if (axis != upflag) {
right_index = 3 - axis - upflag;
/* account for up direction, track direction */
right = right * basis_cross(axis, upflag);
mat.setRow(right_index, right);
mat.setRow(upflag, proj);
mat.setRow(axis, vec);
mat = mat.inverse();
}
/* identity matrix - don't do anything if the two axes are the same */
else {
mat.setIdentity();
}
return mat;
}
void RAS_OpenGLRasterizer::FlushDebugShapes()
{
if (m_debugShapes.empty())
return;
// DrawDebugLines
GLboolean light, tex;
light= glIsEnabled(GL_LIGHTING);
tex= glIsEnabled(GL_TEXTURE_2D);
if (light) glDisable(GL_LIGHTING);
if (tex) glDisable(GL_TEXTURE_2D);
//draw lines
glBegin(GL_LINES);
for (unsigned int i=0;i<m_debugShapes.size();i++)
{
if (m_debugShapes[i].m_type != OglDebugShape::LINE)
continue;
glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f);
const MT_Scalar* fromPtr = &m_debugShapes[i].m_pos.x();
const MT_Scalar* toPtr= &m_debugShapes[i].m_param.x();
glVertex3dv(fromPtr);
glVertex3dv(toPtr);
}
glEnd();
//draw circles
for (unsigned int i=0;i<m_debugShapes.size();i++)
{
if (m_debugShapes[i].m_type != OglDebugShape::CIRCLE)
continue;
glBegin(GL_LINE_LOOP);
glColor4f(m_debugShapes[i].m_color[0],m_debugShapes[i].m_color[1],m_debugShapes[i].m_color[2],1.f);
static const MT_Vector3 worldUp(0.0, 0.0, 1.0);
MT_Vector3 norm = m_debugShapes[i].m_param;
MT_Matrix3x3 tr;
if (norm.fuzzyZero() || norm == worldUp)
{
tr.setIdentity();
}
else
{
MT_Vector3 xaxis, yaxis;
xaxis = MT_cross(norm, worldUp);
yaxis = MT_cross(xaxis, norm);
tr.setValue(xaxis.x(), xaxis.y(), xaxis.z(),
yaxis.x(), yaxis.y(), yaxis.z(),
norm.x(), norm.y(), norm.z());
}
MT_Scalar rad = m_debugShapes[i].m_param2.x();
int n = (int) m_debugShapes[i].m_param2.y();
for (int j = 0; j<n; j++)
{
MT_Scalar theta = j*M_PI*2/n;
MT_Vector3 pos(cos(theta) * rad, sin(theta) * rad, 0.0);
pos = pos*tr;
pos += m_debugShapes[i].m_pos;
const MT_Scalar* posPtr = &pos.x();
glVertex3dv(posPtr);
}
glEnd();
}
if (light) glEnable(GL_LIGHTING);
if (tex) glEnable(GL_TEXTURE_2D);
m_debugShapes.clear();
}
