void
MT_ExpMap::
angleUpdated(
){
m_theta = m_v.length();
reParametrize();
// compute quaternion, sinp and cosp
if (m_theta < MT_EXPMAP_MINANGLE) {
m_sinp = MT_Scalar(0.0);
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * MT_Scalar(MT_Scalar(.5) - m_theta*m_theta/MT_Scalar(48.0));
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(1.0);
} else {
m_sinp = MT_Scalar(sin(.5*m_theta));
/* Taylor Series for sinc */
MT_Vector3 temp = m_v * (m_sinp/m_theta);
m_q.x() = temp.x();
m_q.y() = temp.y();
m_q.z() = temp.z();
m_q.w() = MT_Scalar(cos(.5*m_theta));
}
}
void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle)
{
int nobs = m_obstacles.size();
int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin();
if (obstidx == nobs)
return;
vset(activeObst->dvel, velocity.x(), velocity.y());
//apply RVO
sampleRVO(activeObst, activeNavMeshObj, maxDeltaAngle);
// Fake dynamic constraint.
float dv[2];
float vel[2];
vsub(dv, activeObst->nvel, activeObst->vel);
float ds = vlen(dv);
if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed)
vscale(dv, dv, fabs(maxDeltaSpeed/ds));
vadd(vel, activeObst->vel, dv);
velocity.x() = vel[0];
velocity.y() = vel[1];
}
MT_CmMatrix4x4::MT_CmMatrix4x4(const MT_Point3& orig,
const MT_Vector3& dir,
const MT_Vector3 up)
{
MT_Vector3 z = -(dir.normalized());
MT_Vector3 x = (up.cross(z)).normalized();
MT_Vector3 y = (z.cross(x));
m_V[0][0] = x.x();
m_V[0][1] = y.x();
m_V[0][2] = z.x();
m_V[0][3] = 0.0f;
m_V[1][0] = x.y();
m_V[1][1] = y.y();
m_V[1][2] = z.y();
m_V[1][3] = 0.0f;
m_V[2][0] = x.z();
m_V[2][1] = y.z();
m_V[2][2] = z.z();
m_V[2][3] = 0.0f;
m_V[3][0] = orig.x();//0.0f;
m_V[3][1] = orig.y();//0.0f;
m_V[3][2] = orig.z();//0.0f;
m_V[3][3] = 1.0f;
//Translate(-orig);
}
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();
}
/**
* Computes the intersection between two lines (on the same plane).
* @param vL1 first line vector
* @param pL1 first line point
* @param vL2 second line vector
* @param pL2 second line point
* @param intersection intersection point (if exists)
* @return false if lines are parallels, true otherwise
*/
bool BOP_intersect(const MT_Vector3& vL1, const MT_Point3& pL1, const MT_Vector3& vL2,
const MT_Point3& pL2, MT_Point3 &intersection)
{
// NOTE:
// If the lines aren't on the same plane, the intersection point will not be valid.
// So be careful !!
MT_Scalar t = -1;
MT_Scalar den = (vL1.y()*vL2.x() - vL1.x() * vL2.y());
if (!BOP_fuzzyZero(den)) {
t = (pL2.y()*vL1.x() - vL1.y()*pL2.x() + pL1.x()*vL1.y() - pL1.y()*vL1.x()) / den ;
}
else {
den = (vL1.y()*vL2.z() - vL1.z() * vL2.y());
if (!BOP_fuzzyZero(den)) {
t = (pL2.y()*vL1.z() - vL1.y()*pL2.z() + pL1.z()*vL1.y() - pL1.y()*vL1.z()) / den ;
}
else {
den = (vL1.x()*vL2.z() - vL1.z() * vL2.x());
if (!BOP_fuzzyZero(den)) {
t = (pL2.x()*vL1.z() - vL1.x()*pL2.z() + pL1.z()*vL1.x() - pL1.x()*vL1.z()) / den ;
}
else {
return false;
}
}
}
intersection.setValue(vL2.x()*t + pL2.x(), vL2.y()*t + pL2.y(), vL2.z()*t + pL2.z());
return true;
}
void RAS_CalcTexMatrix(RAS_TexVert p[3],MT_Point3& origin,MT_Vector3& udir,MT_Vector3& vdir)
{
// precondition: 3 vertices are non-colinear
MT_Vector3 vec1 = p[1].xyz()-p[0].xyz();
MT_Vector3 vec2 = p[2].xyz()-p[0].xyz();
MT_Vector3 normal = vec1.cross(vec2);
normal.normalize();
// determine which coordinate we drop, ie. max coordinate in the normal
int ZCOORD = normal.closestAxis();
int XCOORD = (ZCOORD+1)%3;
int YCOORD = (ZCOORD+2)%3;
// ax+by+cz+d=0
MT_Scalar d = -p[0].xyz().dot(normal);
MT_Matrix3x3 mat3( p[0].getUV1()[0],p[0].getUV1()[1], 1,
p[1].getUV1()[0],p[1].getUV1()[1], 1,
p[2].getUV1()[0],p[2].getUV1()[1], 1);
MT_Matrix3x3 mat3inv = mat3.inverse();
MT_Vector3 p123x(p[0].xyz()[XCOORD],p[1].xyz()[XCOORD],p[2].xyz()[XCOORD]);
MT_Vector3 resultx = mat3inv*p123x;
MT_Vector3 p123y(p[0].xyz()[YCOORD],p[1].xyz()[YCOORD],p[2].xyz()[YCOORD]);
MT_Vector3 resulty = mat3inv*p123y;
// normal[ZCOORD] is not zero, because it's chosen to be maximal (absolute), and normal has length 1,
// so at least on of the coords is <> 0
//droppedvalue udir.dot(normal) =0
MT_Scalar droppedu = -(resultx.x()*normal[XCOORD]+resulty.x()*normal[YCOORD])/normal[ZCOORD];
udir[XCOORD] = resultx.x();
udir[YCOORD] = resulty.x();
udir[ZCOORD] = droppedu;
MT_Scalar droppedv = -(resultx.y()*normal[XCOORD]+resulty.y()*normal[YCOORD])/normal[ZCOORD];
vdir[XCOORD] = resultx.y();
vdir[YCOORD] = resulty.y();
vdir[ZCOORD] = droppedv;
// droppedvalue b = -(ax+cz+d)/y;
MT_Scalar droppedvalue = -((resultx.z()*normal[XCOORD] + resulty.z()*normal[YCOORD]+d))/normal[ZCOORD];
origin[XCOORD] = resultx.z();
origin[YCOORD] = resulty.z();
origin[ZCOORD] = droppedvalue;
}
void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v, MT_Scalar norm_weight)
{
m_jacobian[id + 0][dof_id] = v.x() * m_weight_sqrt[dof_id];
m_jacobian[id + 1][dof_id] = v.y() * m_weight_sqrt[dof_id];
m_jacobian[id + 2][dof_id] = v.z() * m_weight_sqrt[dof_id];
m_d_norm_weight[dof_id] = norm_weight;
}
static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
const MT_Vector3& pos1, const MT_Scalar r1,
float& tmin, float& tmax)
{
static const float EPS = 0.0001f;
MT_Vector2 c0(pos0.x(), pos0.y());
MT_Vector2 c1(pos1.x(), pos1.y());
MT_Vector2 s = c1 - c0;
MT_Scalar r = r0+r1;
float c = s.length2() - r*r;
float a = v.length2();
if (a < EPS) return 0; // not moving
// Overlap, calc time to exit.
float b = MT_dot(v,s);
float d = b*b - a*c;
if (d < 0.0f) return 0; // no intersection.
tmin = (b - sqrtf(d)) / a;
tmax = (b + sqrtf(d)) / a;
return 1;
}
/**
* Returns if three points lay on the same line (are collinears).
* @param p1 point
* @param p2 point
* @param p3 point
* @return true if the three points lay on the same line, false otherwise
*/
bool BOP_collinear(const MT_Point3& p1, const MT_Point3& p2, const MT_Point3& p3)
{
if( BOP_comp(p1,p2) == 0 || BOP_comp(p2,p3) == 0 ) return true;
MT_Vector3 v1 = p2 - p1;
MT_Vector3 v2 = p3 - p2;
/* normalize vectors before taking their cross product, so its length
* has some actual meaning */
// if(MT_fuzzyZero(v1.length()) || MT_fuzzyZero(v2.length())) return true;
v1.normalize();
v2.normalize();
MT_Vector3 w = v1.cross(v2);
return (BOP_fuzzyZero(w.x()) && BOP_fuzzyZero(w.y()) && BOP_fuzzyZero(w.z()));
}
MT_Vector3
BSP_GhostTestApp3D::
UnProject(
const MT_Vector3 & vec
) {
GLint viewport[4];
GLdouble mvmatrix[16],projmatrix[16];
glGetIntegerv(GL_VIEWPORT,viewport);
glGetDoublev(GL_MODELVIEW_MATRIX,mvmatrix);
glGetDoublev(GL_PROJECTION_MATRIX,projmatrix);
GLdouble realy = viewport[3] - vec.y() - 1;
GLdouble outx,outy,outz;
gluUnProject(vec.x(),realy,vec.z(),mvmatrix,projmatrix,viewport,&outx,&outy,&outz);
return MT_Vector3(outx,outy,outz);
}
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();
}
void KX_BulletPhysicsController::setScaling(const MT_Vector3& scaling)
{
CcdPhysicsController::setScaling(scaling.x(),scaling.y(),scaling.z());
}
void KX_BulletPhysicsController::SetLinearVelocity(const MT_Vector3& lin_vel,bool local)
{
CcdPhysicsController::SetLinearVelocity(lin_vel.x(),lin_vel.y(),lin_vel.z(),local);
}
void KX_BulletPhysicsController::SetAngularVelocity(const MT_Vector3& ang_vel,bool local)
{
CcdPhysicsController::SetAngularVelocity(ang_vel.x(),ang_vel.y(),ang_vel.z(),local);
}
void KX_BulletPhysicsController::ApplyForce(const MT_Vector3& force,bool local)
{
CcdPhysicsController::ApplyForce(force.x(),force.y(),force.z(),local);
}
bool KX_SoftBodyDeformer::Apply(RAS_IPolyMaterial *polymat, RAS_MeshMaterial *meshmat)
{
CcdPhysicsController *ctrl = (CcdPhysicsController *)m_gameobj->GetPhysicsController();
if (!ctrl)
return false;
btSoftBody *softBody = ctrl->GetSoftBody();
if (!softBody)
return false;
// update the vertex in m_transverts
Update();
RAS_MeshSlot *slot = meshmat->m_slots[(void *)m_gameobj->getClientInfo()];
if (!slot) {
return false;
}
RAS_IDisplayArray *array = slot->GetDisplayArray();
RAS_IDisplayArray *origarray = meshmat->m_baseslot->GetDisplayArray();
btSoftBody::tNodeArray& nodes(softBody->m_nodes);
if (m_needUpdateAabb) {
m_boundingBox->SetAabb(MT_Vector3(0.0f, 0.0f, 0.0f), MT_Vector3(0.0f, 0.0f, 0.0f));
m_needUpdateAabb = false;
}
// AABB Box : min/max.
MT_Vector3 aabbMin;
MT_Vector3 aabbMax;
for (unsigned int i = 0, size = array->GetVertexCount(); i < size; ++i) {
RAS_ITexVert *v = array->GetVertex(i);
const RAS_TexVertInfo& vinfo = origarray->GetVertexInfo(i);
/* The physics converter write the soft body index only in the original
* vertex array because at this moment it doesn't know which is the
* game object. It didn't cause any issues because it's always the same
* vertex order.
*/
const unsigned int softbodyindex = vinfo.getSoftBodyIndex();
MT_Vector3 pt(
nodes[softbodyindex].m_x.getX(),
nodes[softbodyindex].m_x.getY(),
nodes[softbodyindex].m_x.getZ());
v->SetXYZ(pt);
MT_Vector3 normal(
nodes[softbodyindex].m_n.getX(),
nodes[softbodyindex].m_n.getY(),
nodes[softbodyindex].m_n.getZ());
v->SetNormal(normal);
if (!m_gameobj->GetAutoUpdateBounds()) {
continue;
}
const MT_Vector3& scale = m_gameobj->NodeGetWorldScaling();
const MT_Vector3& invertscale = MT_Vector3(1.0f / scale.x(), 1.0f / scale.y(), 1.0f / scale.z());
const MT_Vector3& pos = m_gameobj->NodeGetWorldPosition();
const MT_Matrix3x3& rot = m_gameobj->NodeGetWorldOrientation();
// Extract object transform from the vertex position.
pt = (pt - pos) * rot * invertscale;
// if the AABB need an update.
if (i == 0) {
aabbMin = aabbMax = pt;
}
else {
aabbMin.x() = std::min(aabbMin.x(), pt.x());
aabbMin.y() = std::min(aabbMin.y(), pt.y());
aabbMin.z() = std::min(aabbMin.z(), pt.z());
aabbMax.x() = std::max(aabbMax.x(), pt.x());
aabbMax.y() = std::max(aabbMax.y(), pt.y());
aabbMax.z() = std::max(aabbMax.z(), pt.z());
}
}
array->UpdateFrom(origarray, origarray->GetModifiedFlag() &
(RAS_IDisplayArray::TANGENT_MODIFIED |
RAS_IDisplayArray::UVS_MODIFIED |
RAS_IDisplayArray::COLORS_MODIFIED));
m_boundingBox->ExtendAabb(aabbMin, aabbMax);
return true;
}
LOD_Decimation_InfoPtr
NewVertsFromFile(
char * file_name,
MT_Vector3 &min,
MT_Vector3 &max
) {
min = MT_Vector3(MT_INFINITY,MT_INFINITY,MT_INFINITY);
max = MT_Vector3(-MT_INFINITY,-MT_INFINITY,-MT_INFINITY);
PlyProperty vert_props[] = { /* list of property information for a vertex */
{"x", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,x), 0, 0, 0, 0},
{"y", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,y), 0, 0, 0, 0},
{"z", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,z), 0, 0, 0, 0},
};
PlyProperty face_props[] = { /* list of property information for a vertex */
{"intensity", PLY_UCHAR, PLY_UCHAR, offsetof(LoadFace,intensity), 0, 0, 0, 0},
{"vertex_indices", PLY_INT, PLY_INT, offsetof(LoadFace,verts),
1, PLY_UCHAR, PLY_UCHAR, offsetof(LoadFace,nverts)},
};
#if 0
MEM_SmartPtr<std::vector<float> > verts = new std::vector<float>;
MEM_SmartPtr<std::vector<float> > vertex_normals = new std::vector<float>;
MEM_SmartPtr<std::vector<int> > faces = new std::vector<int>;
#else
std::vector<float>* verts = new std::vector<float>;
std::vector<float>* vertex_normals = new std::vector<float>;
std::vector<int> * faces = new std::vector<int>;
#endif
int i,j;
PlyFile *ply;
int nelems;
char **elist;
int file_type;
float version;
int nprops;
int num_elems;
PlyProperty **plist;
char *elem_name;
LoadVertex load_vertex;
LoadFace load_face;
/* open a PLY file for reading */
ply = ply_open_for_reading(file_name, &nelems, &elist, &file_type, &version);
if (ply == NULL) return NULL;
/* go through each kind of element that we learned is in the file */
/* and read them */
for (i = 0; i < nelems; i++) {
/* get the description of the first element */
elem_name = elist[i];
plist = ply_get_element_description (ply, elem_name, &num_elems, &nprops);
/* print the name of the element, for debugging */
/* if we're on vertex elements, read them in */
if (equal_strings ("vertex", elem_name)) {
/* set up for getting vertex elements */
ply_get_property (ply, elem_name, &vert_props[0]);
ply_get_property (ply, elem_name, &vert_props[1]);
ply_get_property (ply, elem_name, &vert_props[2]);
// make some memory for the vertices
verts->reserve(num_elems);
/* grab all the vertex elements */
for (j = 0; j < num_elems; j++) {
/* grab and element from the file */
ply_get_element (ply, (void *)&load_vertex);
// pass the vertex into the mesh builder.
if (load_vertex.x < min.x()) {
min.x() = load_vertex.x;
} else
if (load_vertex.x > max.x()) {
max.x()= load_vertex.x;
}
if (load_vertex.y < min.y()) {
min.y() = load_vertex.y;
} else
if (load_vertex.y > max.y()) {
max.y()= load_vertex.y;
}
if (load_vertex.z < min.z()) {
min.z() = load_vertex.z;
} else
if (load_vertex.z > max.z()) {
max.z()= load_vertex.z;
}
verts->push_back(load_vertex.x);
verts->push_back(load_vertex.y);
verts->push_back(load_vertex.z);
vertex_normals->push_back(1.0f);
vertex_normals->push_back(0.0f);
vertex_normals->push_back(0.0f);
}
}
/* if we're on face elements, read them in */
if (equal_strings ("face", elem_name)) {
/* set up for getting face elements */
// ply_get_property (ply, elem_name, &face_props[0]);
ply_get_property (ply, elem_name, &face_props[1]);
/* grab all the face elements */
for (j = 0; j < num_elems; j++) {
ply_get_element (ply, (void *)&load_face);
faces->push_back(load_face.verts[0]);
faces->push_back(load_face.verts[1]);
faces->push_back(load_face.verts[2]);
// free up the memory this pile of shit used to allocate the polygon's vertices
free (load_face.verts);
}
}
}
/* close the PLY file */
ply_close (ply);
LOD_Decimation_InfoPtr output = new LOD_Decimation_Info;
output->vertex_buffer = verts->begin();
output->vertex_num = verts->size()/3;
output->triangle_index_buffer = faces->begin();
output->face_num = faces->size()/3;
output->intern = NULL;
output->vertex_normal_buffer = vertex_normals->begin();
// memory leaks 'r' us
#if 0
verts.Release();
vertex_normals.Release();
faces.Release();
#endif
return output;
}
void IK_QJacobian::SetDerivatives(int id, int dof_id, const MT_Vector3& v)
{
m_jacobian[id][dof_id] = v.x()*m_weight_sqrt[dof_id];
m_jacobian[id+1][dof_id] = v.y()*m_weight_sqrt[dof_id];
m_jacobian[id+2][dof_id] = v.z()*m_weight_sqrt[dof_id];
}
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 */
}
void IK_QJacobian::SetBetas(int id, int, const MT_Vector3& v)
{
m_beta[id] = v.x();
m_beta[id+1] = v.y();
m_beta[id+2] = v.z();
}
void KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
{
CcdPhysicsController::ApplyTorque(torque.x(),torque.y(),torque.z(),local);
}
MEM_SmartPtr<BSP_TMesh>
BSP_PlyLoader::
NewMeshFromFile(
char * file_name,
MT_Vector3 &min,
MT_Vector3 &max
) {
min = MT_Vector3(MT_INFINITY,MT_INFINITY,MT_INFINITY);
max = MT_Vector3(-MT_INFINITY,-MT_INFINITY,-MT_INFINITY);
PlyProperty vert_props[] = { /* list of property information for a vertex */
{"x", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,x), 0, 0, 0, 0},
{"y", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,y), 0, 0, 0, 0},
{"z", PLY_FLOAT, PLY_FLOAT, offsetof(LoadVertex,z), 0, 0, 0, 0},
};
PlyProperty face_props[] = { /* list of property information for a vertex */
{"vertex_indices", PLY_INT, PLY_INT, offsetof(LoadFace,verts),
1, PLY_UCHAR, PLY_UCHAR, offsetof(LoadFace,nverts)},
};
MEM_SmartPtr<BSP_TMesh> mesh = new BSP_TMesh;
if (mesh == NULL) return NULL;
int i,j;
PlyFile *ply;
int nelems;
char **elist;
int file_type;
float version;
int nprops;
int num_elems;
PlyProperty **plist;
char *elem_name;
LoadVertex load_vertex;
LoadFace load_face;
/* open a PLY file for reading */
ply = ply_open_for_reading(
file_name,
&nelems,
&elist,
&file_type,
&version
);
if (ply == NULL) return NULL;
/* go through each kind of element that we learned is in the file */
/* and read them */
for (i = 0; i < nelems; i++) {
/* get the description of the first element */
elem_name = elist[i];
plist = ply_get_element_description (ply, elem_name, &num_elems, &nprops);
/* print the name of the element, for debugging */
/* if we're on vertex elements, read them in */
if (equal_strings ("vertex", elem_name)) {
/* set up for getting vertex elements */
ply_get_property (ply, elem_name, &vert_props[0]);
ply_get_property (ply, elem_name, &vert_props[1]);
ply_get_property (ply, elem_name, &vert_props[2]);
// make some memory for the vertices
mesh->VertexSet().reserve(num_elems);
/* grab all the vertex elements */
for (j = 0; j < num_elems; j++) {
/* grab and element from the file */
ply_get_element (ply, (void *)&load_vertex);
// pass the vertex into the mesh builder.
if (load_vertex.x < min.x()) {
min.x() = load_vertex.x;
} else
if (load_vertex.x > max.x()) {
max.x()= load_vertex.x;
}
if (load_vertex.y < min.y()) {
min.y() = load_vertex.y;
} else
if (load_vertex.y > max.y()) {
max.y()= load_vertex.y;
}
if (load_vertex.z < min.z()) {
min.z() = load_vertex.z;
} else
if (load_vertex.z > max.z()) {
max.z()= load_vertex.z;
}
BSP_TVertex my_vert;
my_vert.m_pos = MT_Vector3(load_vertex.x,load_vertex.y,load_vertex.z);
mesh->VertexSet().push_back(my_vert);
}
}
/* if we're on face elements, read them in */
if (equal_strings ("face", elem_name)) {
/* set up for getting face elements */
ply_get_property (ply, elem_name, &face_props[0]);
/* grab all the face elements */
for (j = 0; j < num_elems; j++) {
ply_get_element (ply, (void *)&load_face);
int v;
for (v = 2; v< load_face.nverts; v++) {
BSP_TFace f;
f.m_verts[0] = load_face.verts[0];
f.m_verts[1] = load_face.verts[v-1];
f.m_verts[2] = load_face.verts[v];
mesh->BuildNormal(f);
mesh->FaceSet().push_back(f);
}
// free up the memory this pile of shit used to allocate the polygon's vertices
free (load_face.verts);
}
}
}
/* close the PLY file */
ply_close (ply);
return mesh;
}
static int sweepCircleSegment(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v,
const MT_Vector3& pa, const MT_Vector3& pb, const MT_Scalar sr,
float& tmin, float &tmax)
{
// equation parameters
MT_Vector2 c0(pos0.x(), pos0.y());
MT_Vector2 sa(pa.x(), pa.y());
MT_Vector2 sb(pb.x(), pb.y());
MT_Vector2 L = sb-sa;
MT_Vector2 H = c0-sa;
MT_Scalar radius = r0+sr;
float l2 = L.length2();
float r2 = radius * radius;
float dl = perp(v, L);
float hl = perp(H, L);
float a = dl * dl;
float b = 2.0f * hl * dl;
float c = hl * hl - (r2 * l2);
float d = (b*b) - (4.0f * a * c);
// infinite line missed by infinite ray.
if (d < 0.0f)
return 0;
d = sqrtf(d);
tmin = (-b - d) / (2.0f * a);
tmax = (-b + d) / (2.0f * a);
// line missed by ray range.
/* if (tmax < 0.0f || tmin > 1.0f)
return 0;*/
// find what part of the ray was collided.
MT_Vector2 Pedge;
Pedge = c0+v*tmin;
H = Pedge - sa;
float e0 = MT_dot(H, L) / l2;
Pedge = c0 + v*tmax;
H = Pedge - sa;
float e1 = MT_dot(H, L) / l2;
if (e0 < 0.0f || e1 < 0.0f)
{
float ctmin, ctmax;
if (sweepCircleCircle(pos0, r0, v, pa, sr, ctmin, ctmax))
{
if (e0 < 0.0f && ctmin > tmin)
tmin = ctmin;
if (e1 < 0.0f && ctmax < tmax)
tmax = ctmax;
}
else
{
return 0;
}
}
if (e0 > 1.0f || e1 > 1.0f)
{
float ctmin, ctmax;
if (sweepCircleCircle(pos0, r0, v, pb, sr, ctmin, ctmax))
{
if (e0 > 1.0f && ctmin > tmin)
tmin = ctmin;
if (e1 > 1.0f && ctmax < tmax)
tmax = ctmax;
}
else
{
return 0;
}
}
return 1;
}