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;
}
static float angle(const MT_Vector3& v1, const MT_Vector3& v2)
{
return safe_acos(v1.dot(v2));
}
void KX_BulletPhysicsController::ApplyTorque(const MT_Vector3& torque,bool local)
{
CcdPhysicsController::ApplyTorque(torque.x(),torque.y(),torque.z(),local);
}
bool KX_ConstraintActuator::Update(double curtime, bool frame)
{
bool result = false;
bool bNegativeEvent = IsNegativeEvent();
RemoveAllEvents();
if (!bNegativeEvent) {
/* Constraint clamps the values to the specified range, with a sort of */
/* low-pass filtered time response, if the damp time is unequal to 0. */
/* Having to retrieve location/rotation and setting it afterwards may not */
/* be efficient enough... Something to look at later. */
KX_GameObject *obj = (KX_GameObject*) GetParent();
MT_Point3 position = obj->NodeGetWorldPosition();
MT_Point3 newposition;
MT_Vector3 normal, direction, refDirection;
MT_Matrix3x3 rotation = obj->NodeGetWorldOrientation();
MT_Scalar filter, newdistance, cosangle;
int axis, sign;
if (m_posDampTime) {
filter = m_posDampTime/(1.0+m_posDampTime);
} else {
filter = 0.0;
}
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
case KX_ACT_CONSTRAINT_ORIY:
case KX_ACT_CONSTRAINT_ORIZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
direction[0] = rotation[0][0];
direction[1] = rotation[1][0];
direction[2] = rotation[2][0];
axis = 0;
break;
case KX_ACT_CONSTRAINT_ORIY:
direction[0] = rotation[0][1];
direction[1] = rotation[1][1];
direction[2] = rotation[2][1];
axis = 1;
break;
default:
direction[0] = rotation[0][2];
direction[1] = rotation[1][2];
direction[2] = rotation[2][2];
axis = 2;
break;
}
if ((m_maximumBound < (1.0f-FLT_EPSILON)) || (m_minimumBound < (1.0f-FLT_EPSILON))) {
// reference direction needs to be evaluated
// 1. get the cosine between current direction and target
cosangle = direction.dot(m_refDirVector);
if (cosangle >= (m_maximumBound-FLT_EPSILON) && cosangle <= (m_minimumBound+FLT_EPSILON)) {
// no change to do
result = true;
goto CHECK_TIME;
}
// 2. define a new reference direction
// compute local axis with reference direction as X and
// Y in direction X refDirection plane
MT_Vector3 zaxis = m_refDirVector.cross(direction);
if (MT_fuzzyZero2(zaxis.length2())) {
// direction and refDirection are identical,
// choose any other direction to define plane
if (direction[0] < 0.9999)
zaxis = m_refDirVector.cross(MT_Vector3(1.0,0.0,0.0));
else
zaxis = m_refDirVector.cross(MT_Vector3(0.0,1.0,0.0));
}
MT_Vector3 yaxis = zaxis.cross(m_refDirVector);
yaxis.normalize();
if (cosangle > m_minimumBound) {
// angle is too close to reference direction,
// choose a new reference that is exactly at minimum angle
refDirection = m_minimumBound * m_refDirVector + m_minimumSine * yaxis;
} else {
// angle is too large, choose new reference direction at maximum angle
refDirection = m_maximumBound * m_refDirVector + m_maximumSine * yaxis;
}
} else {
refDirection = m_refDirVector;
}
// apply damping on the direction
direction = filter*direction + (1.0-filter)*refDirection;
obj->AlignAxisToVect(direction, axis);
result = true;
goto CHECK_TIME;
case KX_ACT_CONSTRAINT_DIRPX:
case KX_ACT_CONSTRAINT_DIRPY:
case KX_ACT_CONSTRAINT_DIRPZ:
case KX_ACT_CONSTRAINT_DIRNX:
case KX_ACT_CONSTRAINT_DIRNY:
case KX_ACT_CONSTRAINT_DIRNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
axis = 0; // axis according to KX_GameObject::AlignAxisToVect()
sign = 0; // X axis will be parrallel to direction of ray
break;
case KX_ACT_CONSTRAINT_DIRPY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
axis = 1;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRPZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
axis = 2;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRNX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
axis = 0;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
axis = 1;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
axis = 2;
sign = 1;
break;
}
normal.normalize();
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
// direction of the ray is along the local axis
direction = normal;
} else {
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
direction = MT_Vector3(1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRPY:
direction = MT_Vector3(0.0,1.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRPZ:
direction = MT_Vector3(0.0,0.0,1.0);
break;
case KX_ACT_CONSTRAINT_DIRNX:
direction = MT_Vector3(-1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRNY:
direction = MT_Vector3(0.0,-1.0,0.0);
break;
case KX_ACT_CONSTRAINT_DIRNZ:
direction = MT_Vector3(0.0,0.0,-1.0);
break;
}
}
{
MT_Point3 topoint = position + (m_maximumBound) * direction;
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
std::cout << "WARNING: Constraint actuator " << GetName() << ": There is no physics environment!" << std::endl;
goto CHECK_TIME;
}
if (!spc) {
// the object is not physical, we probably want to avoid hitting its own parent
KX_GameObject *parent = obj->GetParent();
if (parent) {
spc = parent->GetPhysicsController();
}
}
KX_RayCast::Callback<KX_ConstraintActuator> callback(this,dynamic_cast<PHY_IPhysicsController*>(spc));
result = KX_RayCast::RayTest(pe, position, topoint, callback);
if (result) {
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
if ((m_option & (KX_ACT_CONSTRAINT_NORMAL|KX_ACT_CONSTRAINT_DISTANCE)) == 0) {
// if none option is set, the actuator does nothing but detect ray
// (works like a sensor)
goto CHECK_TIME;
}
if (m_option & KX_ACT_CONSTRAINT_NORMAL) {
MT_Scalar rotFilter;
// apply damping on the direction
if (m_rotDampTime) {
rotFilter = m_rotDampTime/(1.0+m_rotDampTime);
} else {
rotFilter = filter;
}
newnormal = rotFilter*normal - (1.0-rotFilter)*newnormal;
obj->AlignAxisToVect((sign)?-newnormal:newnormal, axis);
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
direction = newnormal;
direction.normalize();
}
}
if (m_option & KX_ACT_CONSTRAINT_DISTANCE) {
if (m_posDampTime) {
newdistance = filter*(position-callback.m_hitPoint).length()+(1.0-filter)*m_minimumBound;
} else {
newdistance = m_minimumBound;
}
// logically we should cancel the speed along the ray direction as we set the
// position along that axis
spc = obj->GetPhysicsController();
if (spc && spc->IsDynamic()) {
MT_Vector3 linV = spc->GetLinearVelocity();
// cancel the projection along the ray direction
MT_Scalar fallspeed = linV.dot(direction);
if (!MT_fuzzyZero(fallspeed))
spc->SetLinearVelocity(linV-fallspeed*direction,false);
}
} else {
newdistance = (position-callback.m_hitPoint).length();
}
newposition = callback.m_hitPoint-newdistance*direction;
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
goto CHECK_TIME;
}
}
break;
case KX_ACT_CONSTRAINT_FHPX:
case KX_ACT_CONSTRAINT_FHPY:
case KX_ACT_CONSTRAINT_FHPZ:
case KX_ACT_CONSTRAINT_FHNX:
case KX_ACT_CONSTRAINT_FHNY:
case KX_ACT_CONSTRAINT_FHNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_FHPX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
direction = MT_Vector3(1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHPY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
direction = MT_Vector3(0.0,1.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHPZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
direction = MT_Vector3(0.0,0.0,1.0);
break;
case KX_ACT_CONSTRAINT_FHNX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
direction = MT_Vector3(-1.0,0.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHNY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
direction = MT_Vector3(0.0,-1.0,0.0);
break;
case KX_ACT_CONSTRAINT_FHNZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
direction = MT_Vector3(0.0,0.0,-1.0);
break;
}
normal.normalize();
{
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
std::cout << "WARNING: Constraint actuator " << GetName() << ": There is no physics environment!" << std::endl;
goto CHECK_TIME;
}
if (!spc || !spc->IsDynamic()) {
// the object is not dynamic, it won't support setting speed
goto CHECK_TIME;
}
m_hitObject = NULL;
// distance of Fh area is stored in m_minimum
MT_Point3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
KX_RayCast::Callback<KX_ConstraintActuator> callback(this, spc);
result = KX_RayCast::RayTest(pe, position, topoint, callback);
// we expect a hit object
if (!m_hitObject)
result = false;
if (result)
{
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
MT_Scalar distance = (callback.m_hitPoint-position).length()-spc->GetRadius();
// estimate the velocity of the hit point
MT_Point3 relativeHitPoint;
relativeHitPoint = (callback.m_hitPoint-m_hitObject->NodeGetWorldPosition());
MT_Vector3 velocityHitPoint = m_hitObject->GetVelocity(relativeHitPoint);
MT_Vector3 relativeVelocity = spc->GetLinearVelocity() - velocityHitPoint;
MT_Scalar relativeVelocityRay = direction.dot(relativeVelocity);
MT_Scalar springExtent = 1.0 - distance/m_minimumBound;
// Fh force is stored in m_maximum
MT_Scalar springForce = springExtent * m_maximumBound;
// damping is stored in m_refDirection [0] = damping, [1] = rot damping
MT_Scalar springDamp = relativeVelocityRay * m_refDirVector[0];
MT_Vector3 newVelocity = spc->GetLinearVelocity()-(springForce+springDamp)*direction;
if (m_option & KX_ACT_CONSTRAINT_NORMAL)
{
newVelocity+=(springForce+springDamp)*(newnormal-newnormal.dot(direction)*direction);
}
spc->SetLinearVelocity(newVelocity, false);
if (m_option & KX_ACT_CONSTRAINT_DOROTFH)
{
MT_Vector3 angSpring = (normal.cross(newnormal))*m_maximumBound;
MT_Vector3 angVelocity = spc->GetAngularVelocity();
// remove component that is parallel to normal
angVelocity -= angVelocity.dot(newnormal)*newnormal;
MT_Vector3 angDamp = angVelocity * ((m_refDirVector[1]>MT_EPSILON)?m_refDirVector[1]:m_refDirVector[0]);
spc->SetAngularVelocity(spc->GetAngularVelocity()+(angSpring-angDamp), false);
}
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
}
// don't set the position with this constraint
goto CHECK_TIME;
}
break;
case KX_ACT_CONSTRAINT_LOCX:
case KX_ACT_CONSTRAINT_LOCY:
case KX_ACT_CONSTRAINT_LOCZ:
newposition = position = obj->GetSGNode()->GetLocalPosition();
switch (m_locrot) {
case KX_ACT_CONSTRAINT_LOCX:
Clamp(newposition[0], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCY:
Clamp(newposition[1], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCZ:
Clamp(newposition[2], m_minimumBound, m_maximumBound);
break;
}
result = true;
if (m_posDampTime) {
newposition = filter*position + (1.0-filter)*newposition;
}
obj->NodeSetLocalPosition(newposition);
goto CHECK_TIME;
}
if (result) {
// set the new position but take into account parent if any
obj->NodeSetWorldPosition(newposition);
}
CHECK_TIME:
if (result && m_activeTime > 0 ) {
if (++m_currentTime >= m_activeTime)
result = false;
}
}
if (!result) {
m_currentTime = 0;
}
return result;
} /* end of KX_ConstraintActuator::Update(double curtime,double deltatime) */
bool
LOD_NdQuadricEditor::
BuildQuadrics(
const LOD_ExternNormalEditor& normal_editor,
bool preserve_boundaries
){
if (m_quadrics != NULL) delete(m_quadrics);
m_quadrics =new vector<LOD_NdQuadric> (m_mesh.VertexSet().size());
if (m_quadrics == NULL) return false;
// iterate through the face set of the mesh
// compute a quadric based upon that face and
// add it to each of it's vertices quadrics.
const vector<LOD_TriFace> &faces = m_mesh.FaceSet();
const vector<LOD_Vertex> &verts = m_mesh.VertexSet();
vector<LOD_Edge> &edges = m_mesh.EdgeSet();
const vector<MT_Vector3> &normals = normal_editor.Normals();
vector<MT_Vector3>::const_iterator normal_it = normals.begin();
vector<LOD_TriFace>::const_iterator face_it = faces.begin();
vector<LOD_TriFace>::const_iterator face_end = faces.end();
vector<LOD_NdQuadric> & quadrics = *m_quadrics;
for (; face_it != face_end; ++face_it, ++normal_it) {
MT_Vector3 normal = *normal_it;
MT_Scalar offset = -normal.dot(verts[face_it->m_verts[0]].pos);
LOD_NdQuadric q(normal,offset);
#if 0
// try something with the size of the face.
MT_Vector3 vec1 =
verts[face_it->m_verts[1]].pos -
verts[face_it->m_verts[0]].pos;
MT_Vector3 vec2 =
verts[face_it->m_verts[2]].pos -
verts[face_it->m_verts[1]].pos;
vec1 = vec1.cross(vec2);
q *= vec1.length();
#endif
quadrics[face_it->m_verts[0]] += q;
quadrics[face_it->m_verts[1]] += q;
quadrics[face_it->m_verts[2]] += q;
}
if (preserve_boundaries) {
// iterate through the edge set and add a boundary quadric to
// each of the boundary edges vertices.
vector<LOD_Edge>::const_iterator edge_it = edges.begin();
vector<LOD_Edge>::const_iterator edge_end = edges.end();
for (; edge_it != edge_end; ++edge_it) {
if (edge_it->BoundaryEdge()) {
// compute a plane perpendicular to the edge and the normal
// of the edges single polygon.
const MT_Vector3 & v0 = verts[edge_it->m_verts[0]].pos;
const MT_Vector3 & v1 = verts[edge_it->m_verts[1]].pos;
MT_Vector3 edge_vector = v1 - v0;
LOD_FaceInd edge_face = edge_it->OpFace(LOD_EdgeInd::Empty());
edge_vector = edge_vector.cross(normals[edge_face]);
if (!edge_vector.fuzzyZero()) {
edge_vector.normalize();
LOD_NdQuadric boundary_q(edge_vector, - edge_vector.dot(v0));
boundary_q *= 100;
quadrics[edge_it->m_verts[0]] += boundary_q;
quadrics[edge_it->m_verts[1]] += boundary_q;
}
}
}
}
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 RAS_TexVert::SetTangent(const MT_Vector3& tangent)
{
tangent.getValue(m_tangent);
}
void IKTree::solveJoint(int frame, int i, IKEffectorList &effList)
{
double x, y, z;
double ang = 0;
MT_Quaternion q;
MT_Quaternion totalPosRot = MT_Quaternion(0,0,0,0);
MT_Quaternion totalDirRot = MT_Quaternion(0,0,0,0);
MT_Vector3 axis(0,0,0);
BVHNode *n;
int numPosRot = 0, numDirRot = 0;
if (bone[i].numChildren == 0) { // reached end site
if (bone[i].node->ikOn) {
effList.index[effList.num++] = i;
}
return;
}
for (int j=0; j<bone[i].numChildren; j++) {
IKEffectorList el;
el.num = 0;
solveJoint(frame, bone[i].child[j], el);
for (int k=0; k<el.num; k++) {
effList.index[effList.num++] = el.index[k];
}
}
updateBones(i);
for (int j=0; j<effList.num; j++) {
int effIndex = effList.index[j];
n = bone[effIndex].node;
MT_Vector3 effGoalPos(n->ikGoalPos[0],
n->ikGoalPos[1],
n->ikGoalPos[2]);
const MT_Vector3 pC =
(bone[effIndex].pos - bone[i].pos).safe_normalized();
const MT_Vector3 pD =
(effGoalPos - bone[i].pos).safe_normalized();
MT_Vector3 rotAxis = pC.cross(pD);
if (rotAxis.length2() > MT_EPSILON) {
totalPosRot += MT_Quaternion(rotAxis, bone[i].weight * acos(pC.dot(pD)));
numPosRot++;
}
const MT_Vector3 uC =
(bone[effIndex].pos - bone[effIndex-1].pos).safe_normalized();
const MT_Vector3 uD =
(MT_Vector3(n->ikGoalDir[0], n->ikGoalDir[1], n->ikGoalDir[2])).safe_normalized();
rotAxis = uC.cross(uD);
if (rotAxis.length2() > MT_EPSILON) {
double weight = 0.0;
if (i == effIndex-1) weight = 0.5;
totalDirRot += MT_Quaternion(rotAxis, weight * acos(uC.dot(uD)));
numDirRot++;
}
}
if ((numPosRot + numDirRot) > MT_EPSILON) {
n = bone[i].node;
n->ikOn = true;
// average the quaternions from all effectors
if (numPosRot)
totalPosRot /= numPosRot;
else
totalPosRot = identity;
if (numDirRot)
totalDirRot /= numDirRot;
else
totalDirRot = identity;
MT_Quaternion targetRot = 0.9 * totalPosRot + 0.1 * totalDirRot;
targetRot = targetRot * bone[i].lRot;
toEuler(targetRot, n->channelOrder, x, y, z);
if (jointLimits) {
bone[i].lRot = identity;
for (int k=0; k<n->numChannels; k++) { // clamp each axis in order
switch (n->channelType[k]) {
case BVH_XROT: ang = x; axis = xAxis; break;
case BVH_YROT: ang = y; axis = yAxis; break;
case BVH_ZROT: ang = z; axis = zAxis; break;
default: break;
}
// null axis leads to crash in q.setRotation(), so check first
if(axis.length())
{
if (ang < n->channelMin[k]) ang = n->channelMin[k];
else if (ang > n->channelMax[k]) ang = n->channelMax[k];
q.setRotation(axis, ang * M_PI / 180);
bone[i].lRot = q * bone[i].lRot;
}
}
}
else
bone[i].lRot = targetRot;
}
}
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());
}
bool KX_TrackToActuator::Update(double curtime, bool frame)
{
bool result = false;
bool bNegativeEvent = IsNegativeEvent();
RemoveAllEvents();
if (bNegativeEvent)
{
// do nothing on negative events
}
else if (m_object)
{
KX_GameObject* curobj = (KX_GameObject*) GetParent();
MT_Vector3 dir = ((KX_GameObject*)m_object)->NodeGetWorldPosition() - curobj->NodeGetWorldPosition();
if (dir.length2())
dir.normalize();
MT_Vector3 up(0,0,1);
#ifdef DSADSA
switch (m_upflag)
{
case 0:
{
up.setValue(1.0,0,0);
break;
}
case 1:
{
up.setValue(0,1.0,0);
break;
}
case 2:
default:
{
up.setValue(0,0,1.0);
}
}
#endif
if (m_allow3D)
{
up = (up - up.dot(dir) * dir).safe_normalized();
}
else
{
dir = (dir - up.dot(dir)*up).safe_normalized();
}
MT_Vector3 left;
MT_Matrix3x3 mat;
switch (m_trackflag)
{
case 0: // TRACK X
{
// (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
left = dir.safe_normalized();
dir = (left.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
break;
};
case 1: // TRACK Y
{
// (0.0 , 1.0 , 0.0 ) y direction is forward, z (0.0 , 0.0 , 1.0 ) up
left = (dir.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
break;
}
case 2: // track Z
{
left = up.safe_normalized();
up = dir.safe_normalized();
dir = left;
left = (dir.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
break;
}
case 3: // TRACK -X
{
// (1.0 , 0.0 , 0.0 ) x direction is forward, z (0.0 , 0.0 , 1.0 ) up
left = -dir.safe_normalized();
dir = -(left.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
break;
};
case 4: // TRACK -Y
{
// (0.0 , -1.0 , 0.0 ) -y direction is forward, z (0.0 , 0.0 , 1.0 ) up
left = (-dir.cross(up)).safe_normalized();
mat.setValue (
left[0], -dir[0],up[0],
left[1], -dir[1],up[1],
left[2], -dir[2],up[2]
);
break;
}
case 5: // track -Z
{
left = up.safe_normalized();
up = -dir.safe_normalized();
dir = left;
left = (dir.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
break;
}
default:
{
// (1.0 , 0.0 , 0.0 ) -x direction is forward, z (0.0 , 0.0 , 1.0 ) up
left = -dir.safe_normalized();
dir = -(left.cross(up)).safe_normalized();
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
}
}
MT_Matrix3x3 oldmat;
oldmat= curobj->NodeGetWorldOrientation();
/* erwin should rewrite this! */
mat= matrix3x3_interpol(oldmat, mat, m_time);
if(m_parentobj){ // check if the model is parented and calculate the child transform
MT_Point3 localpos;
localpos = curobj->GetSGNode()->GetLocalPosition();
// Get the inverse of the parent matrix
MT_Matrix3x3 parentmatinv;
parentmatinv = m_parentobj->NodeGetWorldOrientation ().inverse ();
// transform the local coordinate system into the parents system
mat = parentmatinv * mat;
// append the initial parent local rotation matrix
mat = m_parentlocalmat * mat;
// set the models tranformation properties
curobj->NodeSetLocalOrientation(mat);
curobj->NodeSetLocalPosition(localpos);
//curobj->UpdateTransform();
}
else
{
curobj->NodeSetLocalOrientation(mat);
}
result = true;
}
return result;
}
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_RaySensor::Evaluate()
{
bool result = false;
bool reset = m_reset && m_level;
m_rayHit = false;
m_hitObject = NULL;
m_hitPosition[0] = 0;
m_hitPosition[1] = 0;
m_hitPosition[2] = 0;
m_hitNormal[0] = 1;
m_hitNormal[1] = 0;
m_hitNormal[2] = 0;
KX_GameObject* obj = (KX_GameObject*)GetParent();
MT_Point3 frompoint = obj->NodeGetWorldPosition();
MT_Matrix3x3 matje = obj->NodeGetWorldOrientation();
MT_Matrix3x3 invmat = matje.inverse();
MT_Vector3 todir;
m_reset = false;
switch (m_axis)
{
case SENS_RAY_X_AXIS: // X
{
todir[0] = invmat[0][0];
todir[1] = invmat[0][1];
todir[2] = invmat[0][2];
break;
}
case SENS_RAY_Y_AXIS: // Y
{
todir[0] = invmat[1][0];
todir[1] = invmat[1][1];
todir[2] = invmat[1][2];
break;
}
case SENS_RAY_Z_AXIS: // Z
{
todir[0] = invmat[2][0];
todir[1] = invmat[2][1];
todir[2] = invmat[2][2];
break;
}
case SENS_RAY_NEG_X_AXIS: // -X
{
todir[0] = -invmat[0][0];
todir[1] = -invmat[0][1];
todir[2] = -invmat[0][2];
break;
}
case SENS_RAY_NEG_Y_AXIS: // -Y
{
todir[0] = -invmat[1][0];
todir[1] = -invmat[1][1];
todir[2] = -invmat[1][2];
break;
}
case SENS_RAY_NEG_Z_AXIS: // -Z
{
todir[0] = -invmat[2][0];
todir[1] = -invmat[2][1];
todir[2] = -invmat[2][2];
break;
}
}
todir.normalize();
m_rayDirection[0] = todir[0];
m_rayDirection[1] = todir[1];
m_rayDirection[2] = todir[2];
MT_Point3 topoint = frompoint + (m_distance) * todir;
PHY_IPhysicsEnvironment* pe = m_scene->GetPhysicsEnvironment();
if (!pe)
{
std::cout << "WARNING: Ray sensor " << GetName() << ": There is no physics environment!" << std::endl;
std::cout << " Check universe for malfunction." << std::endl;
return false;
}
KX_IPhysicsController *spc = obj->GetPhysicsController();
KX_GameObject *parent = obj->GetParent();
if (!spc && parent)
spc = parent->GetPhysicsController();
if (parent)
parent->Release();
PHY_IPhysicsEnvironment* physics_environment = this->m_scene->GetPhysicsEnvironment();
KX_RayCast::Callback<KX_RaySensor> callback(this, spc);
KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback);
/* now pass this result to some controller */
if (m_rayHit)
{
if (!m_bTriggered)
{
// notify logicsystem that ray is now hitting
result = true;
m_bTriggered = true;
}
else
{
// notify logicsystem that ray is STILL hitting ...
result = false;
}
}
else
{
if (m_bTriggered)
{
m_bTriggered = false;
// notify logicsystem that ray JUST left the Object
result = true;
}
else
{
result = false;
}
}
if (reset)
// force an event
result = true;
return result;
}
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);
}
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;
}
bool KX_SteeringActuator::Update(double curtime, bool frame)
{
if (frame)
{
double delta = curtime - m_updateTime;
m_updateTime = curtime;
if (m_posevent && !m_isActive)
{
delta = 0.0;
m_pathUpdateTime = -1.0;
m_updateTime = curtime;
m_isActive = true;
}
bool bNegativeEvent = IsNegativeEvent();
if (bNegativeEvent)
m_isActive = false;
RemoveAllEvents();
if (!delta)
return true;
if (bNegativeEvent || !m_target)
return false; // do nothing on negative events
KX_GameObject *obj = (KX_GameObject*) GetParent();
const MT_Vector3& mypos = obj->NodeGetWorldPosition();
const MT_Vector3& targpos = m_target->NodeGetWorldPosition();
MT_Vector3 vectotarg = targpos - mypos;
MT_Vector3 vectotarg2d = vectotarg;
vectotarg2d.z() = 0.0f;
m_steerVec = MT_Vector3(0.0f, 0.0f, 0.0f);
bool apply_steerforce = false;
bool terminate = true;
switch (m_mode) {
case KX_STEERING_SEEK:
if (vectotarg2d.length2()>m_distance*m_distance)
{
terminate = false;
m_steerVec = vectotarg;
m_steerVec.normalize();
apply_steerforce = true;
}
break;
case KX_STEERING_FLEE:
if (vectotarg2d.length2()<m_distance*m_distance)
{
terminate = false;
m_steerVec = -vectotarg;
m_steerVec.normalize();
apply_steerforce = true;
}
break;
case KX_STEERING_PATHFOLLOWING:
if (m_navmesh && vectotarg.length2()>m_distance*m_distance)
{
terminate = false;
static const MT_Scalar WAYPOINT_RADIUS(0.25f);
if (m_pathUpdateTime<0 || (m_pathUpdatePeriod>=0 &&
curtime - m_pathUpdateTime>((double)m_pathUpdatePeriod/1000.0)))
{
m_pathUpdateTime = curtime;
m_pathLen = m_navmesh->FindPath(mypos, targpos, m_path, MAX_PATH_LENGTH);
m_wayPointIdx = m_pathLen > 1 ? 1 : -1;
}
if (m_wayPointIdx>0)
{
MT_Vector3 waypoint(&m_path[3*m_wayPointIdx]);
if ((waypoint-mypos).length2()<WAYPOINT_RADIUS*WAYPOINT_RADIUS)
{
m_wayPointIdx++;
if (m_wayPointIdx>=m_pathLen)
{
m_wayPointIdx = -1;
terminate = true;
}
else
waypoint.setValue(&m_path[3*m_wayPointIdx]);
}
m_steerVec = waypoint - mypos;
apply_steerforce = true;
if (m_enableVisualization)
{
//debug draw
static const MT_Vector4 PATH_COLOR(1.0f, 0.0f, 0.0f, 1.0f);
m_navmesh->DrawPath(m_path, m_pathLen, PATH_COLOR);
}
}
}
break;
}
if (apply_steerforce)
{
bool isdyna = obj->IsDynamic();
if (isdyna)
m_steerVec.z() = 0;
if (!m_steerVec.fuzzyZero())
m_steerVec.normalize();
MT_Vector3 newvel = m_velocity * m_steerVec;
//adjust velocity to avoid obstacles
if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/)
{
if (m_enableVisualization)
KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector4(1.0f, 0.0f, 0.0f, 1.0f));
m_simulation->AdjustObstacleVelocity(m_obstacle, m_mode!=KX_STEERING_PATHFOLLOWING ? m_navmesh : NULL,
newvel, m_acceleration*(float)delta, m_turnspeed/(180.0f*(float)(M_PI*delta)));
if (m_enableVisualization)
KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector4(0.0f, 1.0f, 0.0f, 1.0f));
}
HandleActorFace(newvel);
if (isdyna)
{
//temporary solution: set 2D steering velocity directly to obj
//correct way is to apply physical force
MT_Vector3 curvel = obj->GetLinearVelocity();
if (m_lockzvel)
newvel.z() = 0.0f;
else
newvel.z() = curvel.z();
obj->setLinearVelocity(newvel, false);
}
else
{
MT_Vector3 movement = delta*newvel;
obj->ApplyMovement(movement, false);
}
}
else
{
if (m_simulation && m_obstacle)
{
m_obstacle->dvel[0] = 0.f;
m_obstacle->dvel[1] = 0.f;
}
}
if (terminate && m_isSelfTerminated)
return false;
}
return true;
}
void RAS_TexVert::SetNormal(const MT_Vector3& normal)
{
normal.getValue(m_normal);
}
void KX_SteeringActuator::HandleActorFace(MT_Vector3& velocity)
{
if (m_facingMode==0 && (!m_navmesh || !m_normalUp))
return;
KX_GameObject* curobj = (KX_GameObject*) GetParent();
MT_Vector3 dir = m_facingMode==0 ? curobj->NodeGetLocalOrientation().getColumn(1) : velocity;
if (dir.fuzzyZero())
return;
dir.normalize();
MT_Vector3 up(0,0,1);
MT_Vector3 left;
MT_Matrix3x3 mat;
if (m_navmesh && m_normalUp)
{
dtStatNavMesh* navmesh = m_navmesh->GetNavMesh();
MT_Vector3 normal;
MT_Vector3 trpos = m_navmesh->TransformToLocalCoords(curobj->NodeGetWorldPosition());
if (getNavmeshNormal(navmesh, trpos, normal))
{
left = (dir.cross(up)).safe_normalized();
dir = (-left.cross(normal)).safe_normalized();
up = normal;
}
}
switch (m_facingMode)
{
case 1: // TRACK X
{
left = dir.safe_normalized();
dir = -(left.cross(up)).safe_normalized();
break;
};
case 2: // TRACK Y
{
left = (dir.cross(up)).safe_normalized();
break;
}
case 3: // track Z
{
left = up.safe_normalized();
up = dir.safe_normalized();
dir = left;
left = (dir.cross(up)).safe_normalized();
break;
}
case 4: // TRACK -X
{
left = -dir.safe_normalized();
dir = -(left.cross(up)).safe_normalized();
break;
};
case 5: // TRACK -Y
{
left = (-dir.cross(up)).safe_normalized();
dir = -dir;
break;
}
case 6: // track -Z
{
left = up.safe_normalized();
up = -dir.safe_normalized();
dir = left;
left = (dir.cross(up)).safe_normalized();
break;
}
}
mat.setValue (
left[0], dir[0],up[0],
left[1], dir[1],up[1],
left[2], dir[2],up[2]
);
KX_GameObject* parentObject = curobj->GetParent();
if (parentObject)
{
MT_Vector3 localpos;
localpos = curobj->GetSGNode()->GetLocalPosition();
MT_Matrix3x3 parentmatinv;
parentmatinv = parentObject->NodeGetWorldOrientation ().inverse ();
mat = parentmatinv * mat;
mat = m_parentlocalmat * mat;
curobj->NodeSetLocalOrientation(mat);
curobj->NodeSetLocalPosition(localpos);
}
else
{
curobj->NodeSetLocalOrientation(mat);
}
}
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 GPC_RenderTools::applyTransform(RAS_IRasterizer* rasty,double* oglmatrix,int objectdrawmode )
{
/* FIXME:
blender: intern/moto/include/MT_Vector3.inl:42: MT_Vector3 operator/(const
MT_Vector3&, double): Assertion `!MT_fuzzyZero(s)' failed.
Program received signal SIGABRT, Aborted.
[Switching to Thread 16384 (LWP 1519)]
0x40477571 in kill () from /lib/libc.so.6
(gdb) bt
#7 0x08334368 in MT_Vector3::normalized() const ()
#8 0x0833e6ec in GPC_RenderTools::applyTransform(RAS_IRasterizer*, double*, int) ()
*/
if (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED ||
objectdrawmode & RAS_IPolyMaterial::BILLBOARD_AXISALIGNED)
{
// rotate the billboard/halo
//page 360/361 3D Game Engine Design, David Eberly for a discussion
// on screen aligned and axis aligned billboards
// assumed is that the preprocessor transformed all billboard polygons
// so that their normal points into the positive x direction (1.0 , 0.0 , 0.0)
// when new parenting for objects is done, this rotation
// will be moved into the object
MT_Point3 objpos (oglmatrix[12],oglmatrix[13],oglmatrix[14]);
MT_Point3 campos = rasty->GetCameraPosition();
MT_Vector3 dir = (campos - objpos).safe_normalized();
MT_Vector3 up(0,0,1.0);
KX_GameObject* gameobj = (KX_GameObject*) this->m_clientobject;
// get scaling of halo object
MT_Vector3 size = gameobj->GetSGNode()->GetLocalScale();
bool screenaligned = (objectdrawmode & RAS_IPolyMaterial::BILLBOARD_SCREENALIGNED)!=0;//false; //either screen or axisaligned
if (screenaligned)
{
up = (up - up.dot(dir) * dir).safe_normalized();
} else
{
dir = (dir - up.dot(dir)*up).safe_normalized();
}
MT_Vector3 left = dir.normalized();
dir = (left.cross(up)).normalized();
// we have calculated the row vectors, now we keep
// local scaling into account:
left *= size[0];
dir *= size[1];
up *= size[2];
double maat[16]={
left[0], left[1],left[2], 0,
dir[0], dir[1],dir[2],0,
up[0],up[1],up[2],0,
0,0,0,1};
glTranslated(objpos[0],objpos[1],objpos[2]);
glMultMatrixd(maat);
} else
{
if (objectdrawmode & RAS_IPolyMaterial::SHADOW)
{
// shadow must be cast to the ground, physics system needed here!
MT_Point3 frompoint(oglmatrix[12],oglmatrix[13],oglmatrix[14]);
KX_GameObject *gameobj = (KX_GameObject*) this->m_clientobject;
MT_Vector3 direction = MT_Vector3(0,0,-1);
direction.normalize();
direction *= 100000;
MT_Point3 topoint = frompoint + direction;
KX_Scene* kxscene = (KX_Scene*) m_auxilaryClientInfo;
PHY_IPhysicsEnvironment* physics_environment = kxscene->GetPhysicsEnvironment();
KX_IPhysicsController* physics_controller = gameobj->GetPhysicsController();
KX_GameObject *parent = gameobj->GetParent();
if (!physics_controller && parent)
physics_controller = parent->GetPhysicsController();
if (parent)
parent->Release();
KX_RayCast::Callback<GPC_RenderTools> callback(this, physics_controller, oglmatrix);
if (!KX_RayCast::RayTest(physics_environment, frompoint, topoint, callback))
{
// couldn't find something to cast the shadow on...
glMultMatrixd(oglmatrix);
}
} else
{
// 'normal' object
glMultMatrixd(oglmatrix);
}
}
}
/* note, this is called as a python 'getset, where the PyAttributeDef is the closure */
PyObject *PyObjectPlus::py_get_attrdef(PyObject *self_py, const PyAttributeDef *attrdef)
{
PyObjectPlus *ref= (BGE_PROXY_REF(self_py));
char* ptr = (attrdef->m_usePtr) ? (char*)BGE_PROXY_PTR(self_py) : (char*)ref;
if (ptr == NULL || (BGE_PROXY_PYREF(self_py) && (ref==NULL || !ref->py_is_valid()))) {
if (attrdef == BGE_PY_ATTR_INVALID)
Py_RETURN_TRUE; // don't bother running the function
PyErr_SetString(PyExc_SystemError, BGE_PROXY_ERROR_MSG);
return NULL;
}
if (attrdef->m_type == KX_PYATTRIBUTE_TYPE_DUMMY)
{
// fake attribute, ignore
return NULL;
}
if (attrdef->m_type == KX_PYATTRIBUTE_TYPE_FUNCTION)
{
// the attribute has no field correspondence, handover processing to function.
if (attrdef->m_getFunction == NULL)
return NULL;
return (*attrdef->m_getFunction)(ptr, attrdef);
}
ptr += attrdef->m_offset;
if (attrdef->m_length > 1)
{
PyObject *resultlist = PyList_New(attrdef->m_length);
for (unsigned int i=0; i<attrdef->m_length; i++)
{
switch (attrdef->m_type) {
case KX_PYATTRIBUTE_TYPE_BOOL:
{
bool *val = reinterpret_cast<bool*>(ptr);
ptr += sizeof(bool);
PyList_SET_ITEM(resultlist, i, PyBool_FromLong(*val));
break;
}
case KX_PYATTRIBUTE_TYPE_SHORT:
{
short int *val = reinterpret_cast<short int*>(ptr);
ptr += sizeof(short int);
PyList_SET_ITEM(resultlist, i, PyLong_FromLong(*val));
break;
}
case KX_PYATTRIBUTE_TYPE_ENUM:
// enum are like int, just make sure the field size is the same
if (sizeof(int) != attrdef->m_size)
{
Py_DECREF(resultlist);
return NULL;
}
// walkthrough
case KX_PYATTRIBUTE_TYPE_INT:
{
int *val = reinterpret_cast<int*>(ptr);
ptr += sizeof(int);
PyList_SET_ITEM(resultlist, i, PyLong_FromLong(*val));
break;
}
case KX_PYATTRIBUTE_TYPE_FLOAT:
{
float *val = reinterpret_cast<float*>(ptr);
ptr += sizeof(float);
PyList_SET_ITEM(resultlist, i, PyFloat_FromDouble(*val));
break;
}
default:
// no support for array of complex data
Py_DECREF(resultlist);
return NULL;
}
}
return resultlist;
}
else
{
switch (attrdef->m_type) {
case KX_PYATTRIBUTE_TYPE_FLAG:
{
bool bval;
switch (attrdef->m_size) {
case 1:
{
unsigned char *val = reinterpret_cast<unsigned char*>(ptr);
bval = (*val & attrdef->m_imin);
break;
}
case 2:
{
unsigned short *val = reinterpret_cast<unsigned short*>(ptr);
bval = (*val & attrdef->m_imin);
break;
}
case 4:
{
unsigned int *val = reinterpret_cast<unsigned int*>(ptr);
bval = (*val & attrdef->m_imin);
break;
}
default:
return NULL;
}
if (attrdef->m_imax)
bval = !bval;
return PyBool_FromLong(bval);
}
case KX_PYATTRIBUTE_TYPE_BOOL:
{
bool *val = reinterpret_cast<bool*>(ptr);
return PyBool_FromLong(*val);
}
case KX_PYATTRIBUTE_TYPE_SHORT:
{
short int *val = reinterpret_cast<short int*>(ptr);
return PyLong_FromLong(*val);
}
case KX_PYATTRIBUTE_TYPE_ENUM:
// enum are like int, just make sure the field size is the same
if (sizeof(int) != attrdef->m_size)
{
return NULL;
}
// walkthrough
case KX_PYATTRIBUTE_TYPE_INT:
{
int *val = reinterpret_cast<int*>(ptr);
return PyLong_FromLong(*val);
}
case KX_PYATTRIBUTE_TYPE_FLOAT:
{
float *val = reinterpret_cast<float*>(ptr);
if (attrdef->m_imin == 0) {
if (attrdef->m_imax == 0) {
return PyFloat_FromDouble(*val);
} else {
// vector, verify size
if (attrdef->m_size != attrdef->m_imax*sizeof(float))
{
return NULL;
}
#ifdef USE_MATHUTILS
return Vector_CreatePyObject(val, attrdef->m_imax, NULL);
#else
PyObject *resultlist = PyList_New(attrdef->m_imax);
for (unsigned int i=0; i<attrdef->m_imax; i++)
{
PyList_SET_ITEM(resultlist, i, PyFloat_FromDouble(val[i]));
}
return resultlist;
#endif
}
} else {
// matrix case
if (attrdef->m_size != attrdef->m_imax*attrdef->m_imin*sizeof(float))
{
return NULL;
}
#ifdef USE_MATHUTILS
return Matrix_CreatePyObject_wrap(val, attrdef->m_imin, attrdef->m_imax, NULL);
#else
PyObject *collist = PyList_New(attrdef->m_imin);
for (unsigned int i=0; i<attrdef->m_imin; i++)
{
PyObject *col = PyList_New(attrdef->m_imax);
for (unsigned int j=0; j<attrdef->m_imax; j++)
{
PyList_SET_ITEM(col, j, PyFloat_FromDouble(val[j]));
}
PyList_SET_ITEM(collist, i, col);
val += attrdef->m_imax;
}
return collist;
#endif
}
}
case KX_PYATTRIBUTE_TYPE_VECTOR:
{
MT_Vector3 *val = reinterpret_cast<MT_Vector3*>(ptr);
#ifdef USE_MATHUTILS
float fval[3];
val->getValue(fval);
return Vector_CreatePyObject(fval, 3, NULL);
#else
PyObject *resultlist = PyList_New(3);
for (unsigned int i=0; i<3; i++)
{
PyList_SET_ITEM(resultlist, i, PyFloat_FromDouble((*val)[i]));
}
return resultlist;
#endif
}
case KX_PYATTRIBUTE_TYPE_STRING:
{
STR_String *val = reinterpret_cast<STR_String*>(ptr);
return PyUnicode_From_STR_String(*val);
}
case KX_PYATTRIBUTE_TYPE_CHAR:
{
return PyUnicode_FromString(ptr);
}
default:
return NULL;
}
}
}
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();
}
bool KX_ObjectActuator::Update()
{
bool bNegativeEvent = IsNegativeEvent();
RemoveAllEvents();
KX_GameObject *parent = static_cast<KX_GameObject *>(GetParent());
if (bNegativeEvent) {
// If we previously set the linear velocity we now have to inform
// the physics controller that we no longer wish to apply it and that
// it should reconcile the externally set velocity with it's
// own velocity.
if (m_active_combined_velocity) {
if (parent)
parent->ResolveCombinedVelocities(
m_linear_velocity,
m_angular_velocity,
(m_bitLocalFlag.LinearVelocity) != 0,
(m_bitLocalFlag.AngularVelocity) != 0
);
m_active_combined_velocity = false;
}
m_linear_damping_active = false;
m_angular_damping_active = false;
m_error_accumulator.setValue(0.0,0.0,0.0);
m_previous_error.setValue(0.0,0.0,0.0);
return false;
} else if (parent)
{
if (m_bitLocalFlag.ServoControl)
{
// In this mode, we try to reach a target speed using force
// As we don't know the friction, we must implement a generic
// servo control to achieve the speed in a configurable
// v = current velocity
// V = target velocity
// e = V-v = speed error
// dt = time interval since previous update
// I = sum(e(t)*dt)
// dv = e(t) - e(t-1)
// KP, KD, KI : coefficient
// F = KP*e+KI*I+KD*dv
MT_Scalar mass = parent->GetMass();
if (mass < MT_EPSILON)
return false;
MT_Vector3 v = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
if (m_reference)
{
const MT_Point3& mypos = parent->NodeGetWorldPosition();
const MT_Point3& refpos = m_reference->NodeGetWorldPosition();
MT_Point3 relpos;
relpos = (mypos-refpos);
MT_Vector3 vel= m_reference->GetVelocity(relpos);
if (m_bitLocalFlag.LinearVelocity)
// must convert in local space
vel = parent->NodeGetWorldOrientation().transposed()*vel;
v -= vel;
}
MT_Vector3 e = m_linear_velocity - v;
MT_Vector3 dv = e - m_previous_error;
MT_Vector3 I = m_error_accumulator + e;
m_force = m_pid.x()*e+m_pid.y()*I+m_pid.z()*dv;
// to automatically adapt the PID coefficient to mass;
m_force *= mass;
if (m_bitLocalFlag.Torque)
{
if (m_force[0] > m_dloc[0])
{
m_force[0] = m_dloc[0];
I[0] = m_error_accumulator[0];
} else if (m_force[0] < m_drot[0])
{
m_force[0] = m_drot[0];
I[0] = m_error_accumulator[0];
}
}
if (m_bitLocalFlag.DLoc)
{
if (m_force[1] > m_dloc[1])
{
m_force[1] = m_dloc[1];
I[1] = m_error_accumulator[1];
} else if (m_force[1] < m_drot[1])
{
m_force[1] = m_drot[1];
I[1] = m_error_accumulator[1];
}
}
if (m_bitLocalFlag.DRot)
{
if (m_force[2] > m_dloc[2])
{
m_force[2] = m_dloc[2];
I[2] = m_error_accumulator[2];
} else if (m_force[2] < m_drot[2])
{
m_force[2] = m_drot[2];
I[2] = m_error_accumulator[2];
}
}
m_previous_error = e;
m_error_accumulator = I;
parent->ApplyForce(m_force,(m_bitLocalFlag.LinearVelocity) != 0);
} else
{
if (!m_bitLocalFlag.ZeroForce)
{
parent->ApplyForce(m_force,(m_bitLocalFlag.Force) != 0);
}
if (!m_bitLocalFlag.ZeroTorque)
{
parent->ApplyTorque(m_torque,(m_bitLocalFlag.Torque) != 0);
}
if (!m_bitLocalFlag.ZeroDLoc)
{
parent->ApplyMovement(m_dloc,(m_bitLocalFlag.DLoc) != 0);
}
if (!m_bitLocalFlag.ZeroDRot)
{
parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0);
}
if (!m_bitLocalFlag.ZeroLinearVelocity)
{
if (m_bitLocalFlag.AddOrSetLinV) {
parent->addLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
} else {
m_active_combined_velocity = true;
if (m_damping > 0) {
MT_Vector3 linV;
if (!m_linear_damping_active) {
// delta and the start speed (depends on the existing speed in that direction)
linV = parent->GetLinearVelocity(m_bitLocalFlag.LinearVelocity);
// keep only the projection along the desired direction
m_current_linear_factor = linV.dot(m_linear_velocity)/m_linear_length2;
m_linear_damping_active = true;
}
if (m_current_linear_factor < 1.0)
m_current_linear_factor += 1.0/m_damping;
if (m_current_linear_factor > 1.0)
m_current_linear_factor = 1.0;
linV = m_current_linear_factor * m_linear_velocity;
parent->setLinearVelocity(linV,(m_bitLocalFlag.LinearVelocity) != 0);
} else {
parent->setLinearVelocity(m_linear_velocity,(m_bitLocalFlag.LinearVelocity) != 0);
}
}
}
if (!m_bitLocalFlag.ZeroAngularVelocity)
{
m_active_combined_velocity = true;
if (m_damping > 0) {
MT_Vector3 angV;
if (!m_angular_damping_active) {
// delta and the start speed (depends on the existing speed in that direction)
angV = parent->GetAngularVelocity(m_bitLocalFlag.AngularVelocity);
// keep only the projection along the desired direction
m_current_angular_factor = angV.dot(m_angular_velocity)/m_angular_length2;
m_angular_damping_active = true;
}
if (m_current_angular_factor < 1.0)
m_current_angular_factor += 1.0/m_damping;
if (m_current_angular_factor > 1.0)
m_current_angular_factor = 1.0;
angV = m_current_angular_factor * m_angular_velocity;
parent->setAngularVelocity(angV,(m_bitLocalFlag.AngularVelocity) != 0);
} else {
parent->setAngularVelocity(m_angular_velocity,(m_bitLocalFlag.AngularVelocity) != 0);
}
}
}
}
return true;
}
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];
}
void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *>& tasks)
{
// this function will be called before and after solving. calling it before
// solving gives predictable solutions by rotating towards the solution,
// and calling it afterwards ensures the solution is exact.
if (!m_poleconstraint)
return;
// disable pole vector constraint in case of multiple position tasks
std::list<IK_QTask *>::iterator task;
int positiontasks = 0;
for (task = tasks.begin(); task != tasks.end(); task++)
if ((*task)->PositionTask())
positiontasks++;
if (positiontasks >= 2) {
m_poleconstraint = false;
return;
}
// get positions and rotations
root->UpdateTransform(m_rootmatrix);
const MT_Vector3 rootpos = root->GlobalStart();
const MT_Vector3 endpos = m_poletip->GlobalEnd();
const MT_Matrix3x3& rootbasis = root->GlobalTransform().getBasis();
// construct "lookat" matrices (like gluLookAt), based on a direction and
// an up vector, with the direction going from the root to the end effector
// and the up vector going from the root to the pole constraint position.
MT_Vector3 dir = normalize(endpos - rootpos);
MT_Vector3 rootx = rootbasis.getColumn(0);
MT_Vector3 rootz = rootbasis.getColumn(2);
MT_Vector3 up = rootx * cos(m_poleangle) + rootz *sin(m_poleangle);
// in post, don't rotate towards the goal but only correct the pole up
MT_Vector3 poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
MT_Vector3 poleup = normalize(m_polegoal - rootpos);
MT_Matrix3x3 mat, polemat;
mat[0] = normalize(MT_cross(dir, up));
mat[1] = MT_cross(mat[0], dir);
mat[2] = -dir;
polemat[0] = normalize(MT_cross(poledir, poleup));
polemat[1] = MT_cross(polemat[0], poledir);
polemat[2] = -poledir;
if (m_getpoleangle) {
// we compute the pole angle that to rotate towards the target
m_poleangle = angle(mat[1], polemat[1]);
if (rootz.dot(mat[1] * cos(m_poleangle) + mat[0] * sin(m_poleangle)) > 0.0)
m_poleangle = -m_poleangle;
// solve again, with the pole angle we just computed
m_getpoleangle = false;
ConstrainPoleVector(root, tasks);
}
else {
// now we set as root matrix the difference between the current and
// desired rotation based on the pole vector constraint. we use
// transpose instead of inverse because we have orthogonal matrices
// anyway, and in case of a singular matrix we don't get NaN's.
MT_Transform trans(MT_Point3(0, 0, 0), polemat.transposed() * mat);
m_rootmatrix = trans * m_rootmatrix;
}
}
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;
}
