void CDynamics2DEngine::PositionPhysicsToSpace(CVector3& c_new_pos, const CVector3& c_original_pos, const cpBody* pt_body) { c_new_pos.SetX(pt_body->p.x); c_new_pos.SetY(pt_body->p.y); c_new_pos.SetZ(c_original_pos.GetZ()); }
bool CBuzzControllerSpiri::Land() { CVector3 cPos = m_pcPosition->GetReading().Position; if(Abs(cPos.GetZ()) < 0.01f) return false; cPos.SetZ(0.0f); m_pcPropellers->SetAbsolutePosition(cPos); return true; }
// --[ Method ]--------------------------------------------------------------- // // - Class : CMatrix // // - prototype : CMatrix Quaternion() // // - Purpose : Returns the equivalent quaternion. // // ----------------------------------------------------------------------------- CQuaternion CMatrix::Quaternion() const { CQuaternion result; float fTrace, fS; fTrace = 1.0f + m_fM[0][0] + m_fM[1][1] + m_fM[2][2]; // Check trace value if(fTrace > 0.000001f) { fS = 0.5f / sqrtf(fTrace); result.SetW(0.25f / fS); CVector3 v; v.SetX((m_fM[2][1] - m_fM[1][2]) * fS); v.SetY((m_fM[0][2] - m_fM[2][0]) * fS); v.SetZ((m_fM[1][0] - m_fM[0][1]) * fS); result.SetV(v); return result; } else { float qx, qy, qz, qw; if(m_fM[0][0] > m_fM[1][1] && m_fM[0][0] > m_fM[2][2]) { fS = sqrtf(1.0f + m_fM[0][0] - m_fM[1][1] - m_fM[2][2]) * 2.0f; qx = 0.25f * fS; qy = (m_fM[0][1] + m_fM[1][0] ) / fS; qz = (m_fM[0][2] + m_fM[2][0] ) / fS; qw = (m_fM[1][2] - m_fM[2][1] ) / fS; } else if(m_fM[1][1] > m_fM[2][2]) { fS = sqrt( 1.0 + m_fM[1][1] - m_fM[0][0] - m_fM[2][2] ) * 2; qx = (m_fM[0][1] + m_fM[1][0] ) / fS; qy = 0.25f * fS; qz = (m_fM[1][2] + m_fM[2][1] ) / fS; qw = (m_fM[0][2] - m_fM[2][0] ) / fS; } else { fS = sqrt( 1.0 + m_fM[2][2] - m_fM[0][0] - m_fM[1][1] ) * 2; qx = (m_fM[0][2] + m_fM[2][0] ) / fS; qy = (m_fM[1][2] + m_fM[2][1] ) / fS; qz = 0.25f * fS; qw = (m_fM[0][1] - m_fM[1][0] ) / fS; } result.SetW(qw); result.SetV(CVector3(qx, qy, qz)); } return result; }
CVector3 AABox::Corner( int idx ) const { CVector3 rval; rval.SetX( ((idx & 1) == 1) ? mMax.GetX() : mMin.GetX() ); rval.SetY( ((idx & 2) == 1) ? mMax.GetY() : mMin.GetY() ); rval.SetZ( ((idx & 4) == 1) ? mMax.GetZ() : mMin.GetZ() ); return rval; }
CRay3 VoronoiDiagram::ToVoronoiEdge(const Edge& edge) const { CRay3 voronoiEdge; if (edge.is_finite()) { voronoiEdge.SetStart(ToVector3(*edge.vertex0())); voronoiEdge.SetEnd(ToVector3(*edge.vertex1())); } else { const auto& cell1 = *edge.cell(); const auto& cell2 = *edge.twin()->cell(); VoronoiDiagram::Point origin, direction; VoronoiDiagram::Point p1 = boostPoints.at(cell1.source_index()); VoronoiDiagram::Point p2 = boostPoints.at(cell2.source_index()); p1.set(HORIZONTAL, p1.x()/scaleVectorToMilimeters); p1.set(VERTICAL, p1.y()/scaleVectorToMilimeters); p2.set(HORIZONTAL, p2.x()/scaleVectorToMilimeters); p2.set(VERTICAL, p2.y()/scaleVectorToMilimeters); origin.x((p1.x() + p2.x()) * 0.5); origin.y((p1.y() + p2.y()) * 0.5); direction.x(p1.y() - p2.y()); direction.y(p2.x() - p1.x()); Real side = arenaLimits.GetMax().GetX()*2; Real koef = side / max(fabs(direction.x()), fabs(direction.y())); if (edge.vertex0() == NULL) { CVector3 start; start.SetX(origin.x() - (direction.x() * koef)); start.SetY(origin.y() - (direction.y() * koef)); start.SetZ(diagramLiftOnZ); voronoiEdge.SetStart(start); } else { voronoiEdge.SetStart(ToVector3(*edge.vertex0())); } if (edge.vertex1() == NULL) { CVector3 end; end.SetX(origin.x() + direction.x() * koef); end.SetY(origin.y() + direction.y() * koef); end.SetZ(diagramLiftOnZ); voronoiEdge.SetEnd(end); } else { voronoiEdge.SetEnd(ToVector3(*edge.vertex1())); } } return voronoiEdge; }
void AABox::Constrain(CVector3& test_point) const { if (test_point.GetX() > mMax.GetX()) test_point.SetX(mMax.GetX()); else if (test_point.GetX() < mMin.GetX()) test_point.SetX(mMin.GetX()); if (test_point.GetZ() > mMax.GetZ()) test_point.SetZ(mMax.GetZ()); else if (test_point.GetZ() < mMin.GetZ()) test_point.SetZ(mMin.GetZ()); if (test_point.GetY() > mMax.GetY()) test_point.SetY(mMax.GetY()); else if (test_point.GetY() < mMin.GetY()) test_point.SetY(mMin.GetY()); }
// --[ Method ]--------------------------------------------------------------- // // - Class : CMatrix // // - prototype : CVector3 operator * (const CVector3& vector) // // - Purpose : Transforms a given vector3 using the matrix. // // ----------------------------------------------------------------------------- CVector3 CMatrix::operator * (const CVector3& vector) const { CVector3 result; CVector3 v3temp(vector); result.SetX(m_fM[0][0] * v3temp.X() + m_fM[0][1] * v3temp.Y() + m_fM[0][2] * v3temp.Z() + m_fM[0][3]); result.SetY(m_fM[1][0] * v3temp.X() + m_fM[1][1] * v3temp.Y() + m_fM[1][2] * v3temp.Z() + m_fM[1][3]); result.SetZ(m_fM[2][0] * v3temp.X() + m_fM[2][1] * v3temp.Y() + m_fM[2][2] * v3temp.Z() + m_fM[2][3]); return result; }
// --[ Method ]--------------------------------------------------------------- // // - Class : CMatrix // // - prototype : CMatrix Inverse() // // - Purpose : Returns the inverse transformation matrix. // // - Note : IMPORTANT: Algorithm only valid for orthogonal matrices! // // ----------------------------------------------------------------------------- CMatrix CMatrix::Inverse() const { CMatrix result; // Transpose rotation submatrix CVector3 row0(m_fM[0][0], m_fM[1][0], m_fM[2][0]); CVector3 row1(m_fM[0][1], m_fM[1][1], m_fM[2][1]); CVector3 row2(m_fM[0][2], m_fM[1][2], m_fM[2][2]); CVector3 position(m_fM[0][3], m_fM[1][3], m_fM[2][3]); CVector3 invPosition; // Solve ecuation system invPosition.SetX((-row0) * position); invPosition.SetY((-row1) * position); invPosition.SetZ((-row2) * position); // Get scale values CVector3 scale = Scale(); float sqrSclX = scale.X(); sqrSclX *= sqrSclX; float sqrSclY = scale.Y(); sqrSclY *= sqrSclY; float sqrSclZ = scale.Z(); sqrSclZ *= sqrSclZ; // Shouldn't happen: assert(!IS_ZERO(sqrSclX)); assert(!IS_ZERO(sqrSclY)); assert(!IS_ZERO(sqrSclZ)); // Normalize axis and multiply by the inverse scale. row0 = row0 / sqrSclX; row1 = row1 / sqrSclY; row2 = row2 / sqrSclZ; // Insert values result.SetRow0(row0.X(), row0.Y(), row0.Z(), invPosition.X()); result.SetRow1(row1.X(), row1.Y(), row1.Z(), invPosition.Y()); result.SetRow2(row2.X(), row2.Y(), row2.Z(), invPosition.Z()); result.SetRow3( 0.0f, 0.0f, 0.0f, 1.0f); return result; }