MT_Plane3
BSP_CSGMesh::
FacePlane(
const BSP_FaceInd & fi
) const{
const BSP_MFace & f0 = FaceSet()[fi];
// Have to be a bit careful here coz the poly may have
// a lot of parallel edges. Should walk round the polygon
// and check length of cross product.
const MT_Vector3 & p1 = VertexSet()[f0.m_verts[0]].m_pos;
const MT_Vector3 & p2 = VertexSet()[f0.m_verts[1]].m_pos;
int face_size = f0.m_verts.size();
MT_Vector3 n;
for (int i = 2 ; i <face_size; i++) {
const MT_Vector3 & pi = VertexSet()[f0.m_verts[i]].m_pos;
MT_Vector3 l1 = p2-p1;
MT_Vector3 l2 = pi-p2;
n = l1.cross(l2);
MT_Scalar length = n.length();
if (!MT_fuzzyZero(length)) {
n = n * (1/length);
break;
}
}
return MT_Plane3(n,p1);
}
static MT_Vector3 normalize(const MT_Vector3& v)
{
// a sane normalize function that doesn't give (1, 0, 0) in case
// of a zero length vector, like MT_Vector3.normalize
MT_Scalar len = v.length();
return MT_fuzzyZero(len) ? MT_Vector3(0, 0, 0) : v / len;
}
void
init(MT_Vector3 min,MT_Vector3 max)
{
GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 0.5}; /* Red diffuse light. */
GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0}; /* Infinite light location. */
GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 0.5}; /* Red diffuse light. */
GLfloat light_position1[] = {1.0, 0, 0, 0.0}; /* Infinite light location. */
/* Enable a single OpenGL light. */
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0);
glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1);
glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
glEnable(GL_LIGHT0);
// glEnable(GL_LIGHT1);
glEnable(GL_LIGHTING);
// use two sided lighting model
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE,GL_TRUE);
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
/* Setup the view of the cube. */
glMatrixMode(GL_PROJECTION);
// center of the box + 3* depth of box
MT_Vector3 center = (min + max) * 0.5;
MT_Vector3 diag = max - min;
float depth = diag.length();
float distance = 2;
gluPerspective(
/* field of view in degree */ 40.0,
/* aspect ratio */ 1.0,
/* Z near */ 1.0,
/* Z far */ distance * depth * 2
);
glMatrixMode(GL_MODELVIEW);
gluLookAt(
center.x(), center.y(), center.z() + distance*depth, /* eye is at (0,0,5) */
center.x(), center.y(), center.z(), /* center is at (0,0,0) */
0.0, 1.0, 0.); /* up is in positive Y direction */
glPushMatrix();
}
static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R)
{
MT_Vector3 delta = MT_Vector3(R[2][1] - R[1][2],
R[0][2] - R[2][0],
R[1][0] - R[0][1]);
MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1)/2);
MT_Scalar l = delta.length();
if (!MT_fuzzyZero(l))
delta *= c/l;
return delta;
}
void IK_QSegment::SetTransform(
const MT_Vector3& start,
const MT_Matrix3x3& rest_basis,
const MT_Matrix3x3& basis,
const MT_Scalar length
)
{
m_max_extension = start.length() + length;
m_start = start;
m_rest_basis = rest_basis;
m_orig_basis = basis;
SetBasis(basis);
m_translation = MT_Vector3(0, length, 0);
m_orig_translation = m_translation;
}
static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle)
{
switch (obstacle->m_shape)
{
case KX_OBSTACLE_SEGMENT :
{
MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos;
if (!ab.fuzzyZero())
{
MT_Vector3 abdir = ab.normalized();
MT_Vector3 v = pos - obstacle->m_pos;
MT_Scalar proj = abdir.dot(v);
CLAMP(proj, 0, ab.length());
MT_Point3 res = obstacle->m_pos + abdir*proj;
return res;
}
}
case KX_OBSTACLE_CIRCLE :
default:
return obstacle->m_pos;
}
}
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
MT_Vector3 dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id+1);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
MT_Scalar theta = dq.length();
if (!MT_fuzzyZero(theta)) {
MT_Vector3 w = dq*(1.0/theta);
MT_Scalar sine = sin(theta);
MT_Scalar cosine = cos(theta);
MT_Scalar cosineInv = 1-cosine;
MT_Scalar xsine = w.x()*sine;
MT_Scalar zsine = w.z()*sine;
MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
MT_Matrix3x3 M(
cosine + xxcosine, -zsine, xzcosine,
zsine, cosine, -xsine,
xzcosine, xsine, cosine + zzcosine);
m_new_basis = m_basis*M;
RemoveTwist(m_new_basis);
}
else
m_new_basis = m_basis;
if (m_limit_x == false && m_limit_z == false)
return false;
MT_Vector3 a = SphericalRangeParameters(m_new_basis);
MT_Scalar ax = 0, az = 0;
clamp[0] = clamp[1] = false;
if (m_limit_x && m_limit_z) {
ax = a.x();
az = a.z();
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[1] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[1] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[1] = true;
}
}
if (clamp[0] == false && clamp[1] == false)
return false;
m_new_basis = ComputeSwingMatrix(ax, az);
delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
delta[1] = delta[2]; delta[2] = 0.0;
return true;
}
void
BSP_GhostTestApp3D::
InitOpenGl(
const MT_Vector3 &min,
const MT_Vector3 &max
){
GLfloat light_diffuse0[] = {1.0, 0.0, 0.0, 0.5}; /* Red diffuse light. */
GLfloat light_position0[] = {1.0, 1.0, 1.0, 0.0}; /* Infinite light location. */
GLfloat light_diffuse1[] = {1.0, 1.0, 1.0, 0.5}; /* Red diffuse light. */
GLfloat light_position1[] = {1.0, 0, 0, 0.0}; /* Infinite light location. */
/* Enable a single OpenGL light. */
glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse0);
glLightfv(GL_LIGHT0, GL_POSITION, light_position0);
glLightfv(GL_LIGHT1, GL_DIFFUSE, light_diffuse1);
glLightfv(GL_LIGHT1, GL_POSITION, light_position1);
glEnable(GL_LIGHT0);
glEnable(GL_LIGHT1);
glEnable(GL_LIGHTING);
// make sure there is no back face culling.
// glDisable(GL_CULL_FACE);
// use two sided lighting model
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE,GL_TRUE);
/* Use depth buffering for hidden surface elimination. */
glEnable(GL_DEPTH_TEST);
/* Setup the view of the cube. */
glMatrixMode(GL_PROJECTION);
// center of the box + 3* depth of box
MT_Vector3 center = (min + max) * 0.5;
MT_Vector3 diag = max - min;
float depth = diag.length();
float distance = 5;
gluPerspective(
/* field of view in degree */ 40.0,
/* aspect ratio */ 1.0,
/* Z near */ 1.0,
/* Z far */ distance * depth * 2
);
glMatrixMode(GL_MODELVIEW);
gluLookAt(
center.x(), center.y(), center.z() + distance*depth, //eye
center.x(), center.y(), center.z(), //center
0.0, 1.0, 0.
); /* up is in positive Y direction */
}