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; }