static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle) { switch (obstacle->m_shape) { case KX_OBSTACLE_SEGMENT : { MT_Vector3 ab = obstacle->m_pos2 - obstacle->m_pos; if (!ab.fuzzyZero()) { MT_Vector3 abdir = ab.normalized(); MT_Vector3 v = pos - obstacle->m_pos; MT_Scalar proj = abdir.dot(v); CLAMP(proj, 0, ab.length()); MT_Point3 res = obstacle->m_pos + abdir*proj; return res; } } case KX_OBSTACLE_CIRCLE : default: return obstacle->m_pos; } }
bool KX_ObjectActuator::Update() { bool bNegativeEvent = IsNegativeEvent(); RemoveAllEvents(); KX_GameObject *parent = static_cast<KX_GameObject *>(GetParent()); PHY_ICharacter *character = parent->GetScene()->GetPhysicsEnvironment()->GetCharacterController(parent); 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; } // Explicitly stop the movement if we're using character motion if (m_bitLocalFlag.CharacterMotion) { character->SetWalkDirection(MT_Vector3 (0.0f, 0.0f, 0.0f)); } m_linear_damping_active = false; m_angular_damping_active = false; m_error_accumulator.setValue(0.0f,0.0f,0.0f); m_previous_error.setValue(0.0f,0.0f,0.0f); m_jumping = false; 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.CharacterMotion) { MT_Vector3 dir = m_dloc; if (m_bitLocalFlag.DLoc) { MT_Matrix3x3 basis = parent->GetPhysicsController()->GetOrientation(); dir = basis * dir; } if (m_bitLocalFlag.AddOrSetCharLoc) { MT_Vector3 old_dir = character->GetWalkDirection(); if (!old_dir.fuzzyZero()) { MT_Scalar mag = old_dir.length(); dir = dir + old_dir; if (!dir.fuzzyZero()) dir = dir.normalized() * mag; } } // We always want to set the walk direction since a walk direction of (0, 0, 0) should stop the character character->SetWalkDirection(dir/parent->GetScene()->GetPhysicsEnvironment()->GetNumTimeSubSteps()); if (!m_bitLocalFlag.ZeroDRot) { parent->ApplyRotation(m_drot,(m_bitLocalFlag.DRot) != 0); } if (m_bitLocalFlag.CharacterJump) { if (!m_jumping) { character->Jump(); m_jumping = true; } else if (character->OnGround()) m_jumping = false; } } 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) { /* No need to select local or world, as the velocity is zero anyway, * and setLinearVelocity() converts local to world first. We do need to * pass a true zero vector, as m_linear_velocity is only fuzzily zero. */ parent->setLinearVelocity(MT_Vector3(0, 0, 0), false); } } else { 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.0f) m_current_linear_factor += 1.0f/m_damping; if (m_current_linear_factor > 1.0f) m_current_linear_factor = 1.0f; 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) { /* No need to select local or world, as the velocity is zero anyway, * and setAngularVelocity() converts local to world first. We do need to * pass a true zero vector, as m_angular_velocity is only fuzzily zero. */ parent->setAngularVelocity(MT_Vector3(0, 0, 0), false); } else { 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.0f) m_current_angular_factor += 1.0f/m_damping; if (m_current_angular_factor > 1.0f) m_current_angular_factor = 1.0f; 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 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_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); } }