void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex) { CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); CustomControllerConvexRayFilter filter(m_body); NewtonWorldConvexRayCast (world, m_castingShape, &castMatrix[0][0], &dst[0], CustomControllerConvexRayFilter::Filter, &filter, CustomControllerConvexRayFilter::Prefilter, threadIndex); m_groundPlane = dVector (0.0f, 0.0f, 0.0f, 0.0f); m_groundVelocity = dVector (0.0f, 0.0f, 0.0f, 0.0f); if (filter.m_hitBody) { m_isJumping = false; dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (filter.m_intersectParam)); m_groundPlane = filter.m_hitNormal; m_groundPlane.m_w = - (supportPoint % filter.m_hitNormal); NewtonBodyGetPointVelocity (filter.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]); matrix.m_posit = supportPoint; matrix.m_posit.m_w = 1.0f; } }
SupportFunctionDataPtr SupportFunctionDataConstructor::run( std::vector<Point_3> directions, Polyhedron_3 polyhedron) { DEBUG_START; std::vector<Vector_3> vertices = polyhedron.getVertices(); std::vector<SupportFunctionDataItem> items; for (auto &direction: directions) { Vector_3 vDirection = direction - CGAL::ORIGIN; double scalarProductMax = 0.; Vector_3 supportPoint(0., 0., 0.); int iVertexTangient = 0; int iVertex = 0; for (auto &vertex: vertices) { double scalarProduct = vDirection * vertex; if (scalarProduct > scalarProductMax) { scalarProductMax = scalarProduct; supportPoint = vertex; iVertexTangient = iVertex; } ++iVertex; } tangientIDs_.push_back(iVertexTangient); ASSERT(scalarProductMax > 0); SupportFunctionDataItem item(direction, scalarProductMax); item.info = SupportFunctionDataItemInfoPtr( new SupportFunctionDataItemInfo()); item.info->point = supportPoint; items.push_back(item); } DEBUG_END; return SupportFunctionDataPtr(new SupportFunctionData(items)); }
void CustomPlayerController::UpdateGroundPlane (dMatrix& matrix, const dMatrix& castMatrix, const dVector& dst, int threadIndex) { CustomPlayerControllerManager* const manager = (CustomPlayerControllerManager*) GetManager(); NewtonWorld* const world = manager->GetWorld(); NewtonWorldConvexCastReturnInfo info; CustomControllerConvexRayFilter filter(m_body); dFloat param = 10.0f; int count = NewtonWorldConvexCast (world, &castMatrix[0][0], &dst[0], m_castingShape, ¶m, &filter, CustomControllerConvexCastPreFilter::Prefilter, &info, 1, threadIndex); m_groundPlane = dVector (0.0f); m_groundVelocity = dVector (0.0f); if (count && (param <= 1.0f)) { m_isJumping = false; dVector supportPoint (castMatrix.m_posit + (dst - castMatrix.m_posit).Scale (param)); m_groundPlane = dVector (info.m_normal[0], info.m_normal[1], info.m_normal[2], 0.0f); m_groundPlane.m_w = - supportPoint.DotProduct3(m_groundPlane); NewtonBodyGetPointVelocity (info.m_hitBody, &supportPoint.m_x, &m_groundVelocity[0]); matrix.m_posit = supportPoint; matrix.m_posit.m_w = 1.0f; } }