vector<VectorF> getSplineLoopPoints(vector<CubicSpline> splines) { if(splines.size() == 0) return vector<VectorF>(); const int pointsPerSpline = 20; vector<VectorF> points; points.reserve(1 + pointsPerSpline * splines.size()); points.push_back(splines[0].evaluate(0)); for(size_t i = 0; i < splines.size(); i++) { for(int j = 1; j <= pointsPerSpline; j++) { points.push_back(splines[i].evaluate((float)j / pointsPerSpline)); } } for(size_t i = 0; i < points.size() && points.size() > 1;) { size_t j = (i + 1) % points.size(); if(absSquared(points[i] - points[j]) < eps * eps) { points.erase(points.begin() + j); if(j == 0) break; } else i++; } if(points.size() <= 1) return vector<VectorF>(); return std::move(points); }
bool linesIntersect(VectorF start1, VectorF end1, VectorF start2, VectorF end2, float endSpace) { #if 0 float divisor = (start2.x * start1.z - end2.x * start1.z - start2.x * end1.z + end2.x * end1.z - start1.x * start2.z + end1.x * start2.z + start1.x * end2.z - end1.x * end2.z); if(abs(divisor) < eps * eps || abs(end2.z - start2.z) < eps) { return false; } float t1 = (-start2.x * start1.z + end2.x * start1.z + start1.x * start2.z - end2.x * start2.z - start1.x * end2.z + start2.x * end2.z) / -divisor; float t2 = (-end1.x * start1.z + start2.x * start1.z + start1.x * end1.z - start2.x * end1.z - start1.x * start2.z + end1.x * start2.z) / divisor; if(t1 < endSpace || t1 > 1 - endSpace || t2 < endSpace || t2 > 1 - endSpace) { return false; } return absSquared(interpolate(t1, start1, end1) - interpolate(t2, start2, end2)) < eps * eps; #else if(endSpace > 0) { pair<VectorF, VectorF> line1 = make_pair(interpolate(endSpace, start1, end1), interpolate(endSpace, end1, start1)); pair<VectorF, VectorF> line2 = make_pair(interpolate(endSpace, start2, end2), interpolate(endSpace, end2, start2)); start1 = get<0>(line1); end1 = get<1>(line1); start2 = get<0>(line2); end2 = get<1>(line2); } return CCW(start1, start2, end2) != CCW(end1, start2, end2) && CCW(start1, end1, start2) != CCW(start1, end1, end2); #endif }
Mesh Generate::line(const vector<VectorF> &linePointsIn, TextureDescriptor texture, Color color, float lineWidth) { if(linePointsIn.size() < 2) // if less than 2 points then we can't have a line { return Mesh(new Mesh_t()); } vector<VectorF> linePoints; linePoints.reserve(linePointsIn.size()); linePoints.push_back(linePointsIn[0]); for(size_t i = 1; i < linePointsIn.size(); i++) { if(absSquared(linePointsIn[i] - linePoints.back()) >= eps * eps) // remove duplicates linePoints.push_back(linePointsIn[i]); } if(linePoints.size() < 2) // if less than 2 points then we can't have a line { return Mesh(new Mesh_t()); } vector<Edge> edges; edges.reserve(linePoints.size()); float distance = 0; edges.push_back(makeStartEdge(linePoints[0], linePoints[1], distance, lineWidth)); distance += abs(linePoints[1] - linePoints[0]); for(size_t i = 2; i < linePoints.size(); i++) { edges.push_back(makeMiddleEdge(linePoints[i - 2], linePoints[i - 1], linePoints[i], distance, lineWidth)); distance += abs(linePoints[i - 1] - linePoints[i]); } edges.push_back(makeEndEdge(linePoints[linePoints.size() - 2], linePoints[linePoints.size() - 1], distance, lineWidth)); Mesh retval = nullptr; for(size_t i = 1; i < edges.size(); i++) { TextureDescriptor currentTexture = texture.subTexture(edges[i - 1].distance / distance, edges[i].distance / distance, 0, 1); Mesh mesh = Generate::quadrilateral(currentTexture, edges[i - 1].p2, color, edges[i].p2, color, edges[i].p1, color, edges[i - 1].p1, color); if(retval == nullptr) retval = mesh; else retval->add(mesh); } return retval; }
float Vector2f::abs() const { return sqrt(absSquared()); }
bool CubicSpline::intersects(const CubicSpline &rt, VectorF ignoreAxis) const { #if 1 vector<VectorF> path1 = getSplinePoints(*this); vector<VectorF> path2 = getSplinePoints(rt); for(size_t i = 1; i < path1.size(); i++) { for(size_t j = 1; j < path2.size(); j++) { if(linesIntersect(path1[i - 1], path1[i], path2[j - 1], path2[j], 0)) return true; } } return false; #else const int splitCount = 50; // number of line segments to split spline into ignoreAxis = normalize(ignoreAxis); for(int segmentNumber = 0; segmentNumber < splitCount; segmentNumber++) { float startT = (float)(segmentNumber) / splitCount; float endT = (float)(segmentNumber + 1) / splitCount; VectorF startP = rt.evaluate(startT); startP -= ignoreAxis * dot(ignoreAxis, startP); // move to plane normal to ignoreAxis VectorF endP = rt.evaluate(endT); endP -= ignoreAxis * dot(ignoreAxis, endP); // move to plane normal to ignoreAxis VectorF delta = endP - startP; if(absSquared(delta) < eps * eps) // if delta is too small { continue; } // solve dot(evaluate(t), cross(ignoreAxis, delta)) == 0 and it intersects if dot(evaluate(t) - startP, delta) / dot(delta, delta) is in [0, 1] and t is in [0, 1] VectorF normal = cross(ignoreAxis, delta); float cubic = dot(getCubic(), normal); float quadratic = dot(getQuadratic(), normal); float linear = dot(getLinear(), normal); float constant = dot(getConstant(), normal); float intersections[3]; int intersectionCount = solveCubic(constant, linear, quadratic, cubic, intersections); for(int i = 0; i < intersectionCount; i++) { float t = intersections[i]; if(t < 0 || t > 1) { continue; } float v = dot(evaluate(t) - startP, delta) / dot(delta, delta); if(v < 0 || v > 1) { continue; } return true; } } return false; #endif }
void EntityPlayer::onMove(EntityData & data, shared_ptr<World> world, float deltaTimeIn) const { getEntity(data, world); assert(data.extraData); auto eData = dynamic_pointer_cast<ExtraData>(data.extraData); assert(eData); data.deltaAcceleration = VectorF(0); data.acceleration = gravityVector; if(eData->flying) data.acceleration = VectorF(0); int count = iceil(deltaTimeIn * abs(data.velocity) / 0.5 + 1); BlockIterator bi = world->get((PositionI)data.position); data.entity->acceleration = data.acceleration; data.entity->deltaAcceleration = data.deltaAcceleration; auto pphysicsObject = make_shared<PhysicsBox>((VectorF)data.position + physicsOffset(), physicsExtents(), data.velocity, data.entity->acceleration, data.entity->deltaAcceleration, data.position.d, physicsProperties(), -physicsOffset()); PhysicsBox & physicsObject = *pphysicsObject; for(int step = 0; step < count; step++) { float deltaTime = deltaTimeIn / count; data.entity->age += deltaTime; int zeroCount = 0; while(deltaTime * deltaTime * absSquared(data.velocity) > eps * eps) { bool supported = false; PhysicsCollision firstCollision(data.position + deltaTime * data.velocity + deltaTime * deltaTime * 0.5f * data.entity->acceleration + deltaTime * deltaTime * deltaTime * (1 / 6.0f) * data.entity->deltaAcceleration, data.velocity + deltaTime * data.entity->acceleration + deltaTime * deltaTime * 0.5f * data.entity->deltaAcceleration, VectorF(0), deltaTime); physicsObject.reInit((VectorF)data.position + physicsOffset(), physicsExtents(), data.velocity, data.entity->acceleration, data.entity->deltaAcceleration); for(int dx = -1; dx <= 1; dx++) { for(int dy = -2; dy <= 2; dy++) { for(int dz = -1; dz <= 1; dz++) { BlockIterator curBI = bi; curBI += VectorI(dx, dy, dz); shared_ptr<PhysicsObject> otherObject; if(curBI.get().good()) otherObject = curBI.get().desc->getPhysicsObject(curBI.position()); else otherObject = static_pointer_cast<PhysicsObject>(make_shared<PhysicsBox>((VectorI)curBI.position() + VectorF(0.5), VectorF(0.5), VectorF(0), VectorF(0), VectorF(0), curBI.position().d, PhysicsProperties(PhysicsProperties::INFINITE_MASS, 1, 0))); assert(otherObject); { bool filled = false; float newY; switch(otherObject->type()) { case PhysicsObject::Type::Box: { const PhysicsBox * pbox = dynamic_cast<const PhysicsBox *>(otherObject.get()); VectorF min = pbox->center - pbox->extents; VectorF max = pbox->center + pbox->extents; if(min.x <= curBI.position().x && max.x >= curBI.position().x + 1 && min.y <= curBI.position().y && max.y >= curBI.position().y + 1 && min.z <= curBI.position().z && max.z >= curBI.position().z + 1) { newY = max.y + physicsObject.extents.y - physicsOffset().y; filled = true; } VectorF temp; if(isBoxCollision(pbox->center, pbox->extents, physicsObject.center - VectorF(0, eps * 10, 0) + physicsOffset(), physicsObject.extents, temp) && !isBoxCollision(pbox->center, pbox->extents, physicsObject.center + physicsOffset(), physicsObject.extents, temp)) { supported = true; } break; } case PhysicsObject::Type::None: break; } if(filled && zeroCount >= 2 && dx == 0 && dy == 0 && dz == 0) { firstCollision.time = 0; firstCollision.newPosition = data.position; firstCollision.newPosition.y = newY; firstCollision.newVelocity = VectorF(0); firstCollision.collisionNormal = VectorF(0, 1, 0); break; } } PhysicsCollision collision = physicsObject.collide(otherObject, deltaTime); if(collision.valid) { if(collision.time < eps) { if(zeroCount > 25) collision.valid = false; else zeroCount++; } else zeroCount = 0; } if(collision.valid && collision.time < firstCollision.time) firstCollision = collision; } } } deltaTime -= firstCollision.time; data.setPosition(firstCollision.newPosition + eps * (2 + abs(firstCollision.newVelocity)) * firstCollision.collisionNormal); data.setVelocity(firstCollision.newVelocity); data.setAcceleration(data.entity->acceleration + data.entity->deltaAcceleration * firstCollision.time); } } if(eData->pclient == nullptr || !isClientValid(*eData->pclient)) { data.clear(); return; } }