//! Move by polyline and get position on polyline based on distance
void Polygon3::InterpolatePositionFromDistanceReverse(float32 distance, int startSegment, Vector3 & resultPosition, int & resultSegmentIndex) const
{
float32 currentDistance = distance;
int currentSegment = 0;
float currentSegmentLenght = 0.0f;
for (currentSegment = startSegment; currentSegment >= 1; --currentSegment)
{
Vector3 v = points[currentSegment] - points[currentSegment - 1];
currentSegmentLenght = v.Length();
if (currentDistance - currentSegmentLenght < 0.0f)
break; // we've found right segment
currentDistance -= currentSegmentLenght;
}
resultSegmentIndex = -1;
if (currentSegment == 0)
{
return;
}
float t = currentDistance / currentSegmentLenght;
if ((t >= 0.0f) && (t <= 1.0f))
{
resultPosition.Lerp(points[currentSegment], points[currentSegment - 1], t);
resultSegmentIndex = currentSegment;
}
}
Vector3 NavigationMesh::Raycast(const Vector3& start, const Vector3& end, const Vector3& extents, const dtQueryFilter* filter,
Vector3* hitNormal)
{
if (hitNormal)
*hitNormal = Vector3::DOWN;
if (!InitializeQuery())
return end;
const Matrix3x4& transform = node_->GetWorldTransform();
Matrix3x4 inverse = transform.Inverse();
Vector3 localStart = inverse * start;
Vector3 localEnd = inverse * end;
const dtQueryFilter* queryFilter = filter ? filter : queryFilter_.Get();
dtPolyRef startRef;
navMeshQuery_->findNearestPoly(&localStart.x_, &extents.x_, queryFilter, &startRef, 0);
if (!startRef)
return end;
Vector3 normal;
if (!hitNormal)
hitNormal = &normal;
float t;
int numPolys;
navMeshQuery_->raycast(startRef, &localStart.x_, &localEnd.x_, queryFilter, &t, &hitNormal->x_, pathData_->polys_, &numPolys,
MAX_POLYS);
if (t == FLT_MAX)
t = 1.0f;
return start.Lerp(end, t);
}
//! Clip poly by plane
void Polygon3::ClipByPlane(const Plane & plane)
{
// TODO: test / check performance / optimize / эта пиздец! count чему в начале равен?
Vector<Vector3> resultVector;
int32 i2;
int32 count = 0;
for(int32 i = 0; i < pointCount; ++i)
{
i2 = i + 1;
if (i2 >= pointCount) i2 = 0;
float32 d1 = plane.DistanceToPoint(points[i]);
float32 d2 = plane.DistanceToPoint(points[i2]);
if ((d1 >= POLYGON_EPSILON) && (d2 >= POLYGON_EPSILON))
{
resultVector.push_back(points[i]);
resultVector.push_back(points[i2]);
count += 2;
} else if ((d1 >= POLYGON_EPSILON) && (d2 <= -POLYGON_EPSILON))
{
Vector3 res;
res.Lerp(points[i], points[i2], d1 / (d1 - d2));
resultVector.push_back(points[i]);
resultVector.push_back(res);
count += 2;
} else if ((d1 <= -POLYGON_EPSILON) && (d2 >= POLYGON_EPSILON))
{
Vector3 res;
res.Lerp(points[i], points[i2], -d1 / (-d1 + d2));
resultVector.push_back(res);
//resultVector.push_back(points[i]);
count += 1;
}
}
points.clear();
for (int32 i = 0; i < count; ++i)
points.push_back(resultVector[i]);
}
void SmoothedTransform::Update(float constant, float squaredSnapThreshold)
{
if (smoothingMask_ && node_)
{
Vector3 position = node_->GetPosition();
Quaternion rotation = node_->GetRotation();
if (smoothingMask_ & SMOOTH_POSITION)
{
// If position snaps, snap everything to the end
float delta = (position - targetPosition_).LengthSquared();
if (delta > squaredSnapThreshold)
constant = 1.0f;
if (delta < M_EPSILON || constant >= 1.0f)
{
position = targetPosition_;
smoothingMask_ &= ~SMOOTH_POSITION;
}
else
position = position.Lerp(targetPosition_, constant);
node_->SetPosition(position);
}
if (smoothingMask_ & SMOOTH_ROTATION)
{
float delta = (rotation - targetRotation_).LengthSquared();
if (delta < M_EPSILON || constant >= 1.0f)
{
rotation = targetRotation_;
smoothingMask_ &= ~SMOOTH_ROTATION;
}
else
rotation = rotation.Slerp(targetRotation_, constant);
node_->SetRotation(rotation);
}
}
// If smoothing has completed, unsubscribe from the update event
if (!smoothingMask_)
{
UnsubscribeFromEvent(GetScene(), E_UPDATESMOOTHING);
subscribed_ = false;
}
}
void SceneNode::Update(float32 timeElapsed)
{
inUpdate = true;
// TODO - move node update to render because any of objects can change params of other objects
if (nodeAnimations.size() != 0)
{
Quaternion blendedRotation;
Vector3 blendedTranslation;
float32 accumWeight = 0.0f;
std::deque<SceneNodeAnimation*>::const_iterator end = nodeAnimations.end();
for (std::deque<SceneNodeAnimation*>::iterator it = nodeAnimations.begin(); it != end; ++it)
{
SceneNodeAnimation * animation = *it;
SceneNodeAnimationKey & key = animation->Intepolate(animation->GetCurrentTime());
if (accumWeight == 0.0f)
{
blendedTranslation = key.translation;
blendedRotation = key.rotation;
accumWeight = animation->weight;
}else
{
float32 factor = animation->weight / (accumWeight + animation->weight);
accumWeight += accumWeight;
blendedTranslation.Lerp(blendedTranslation, key.translation, factor);
blendedRotation.Slerp(blendedRotation, key.rotation, factor);
}
//key.GetMatrix(localTransform);
}
Matrix4 localTransformTrans;
Matrix4 localTransformRot;
Matrix4 localTransformFinal;
localTransformTrans.CreateTranslation(blendedTranslation);
localTransformRot = blendedRotation.GetMatrix();
localTransform = localTransformRot * localTransformTrans;
// if (nodeAnimations.size() != 1)
// {
// printf("-- blended node: %s\n", name.c_str());
// std::deque<SceneNodeAnimation*>::const_iterator end = nodeAnimations.end();
// for (std::deque<SceneNodeAnimation*>::iterator it = nodeAnimations.begin(); it != end; ++it)
// {
// SceneNodeAnimation * animation = *it;
// printf(">>> blend: %s wei: %f inDelay: %f\n", animation->GetParent()->name.c_str(), animation->weight, animation->delayTime);
// }
// }
}
// update world transform only in case if
if (!(flags & NODE_WORLD_MATRIX_ACTUAL))
{
if (parent)
{
worldTransform = localTransform * parent->worldTransform;
}else
{
worldTransform = localTransform;
}
// need propagate changes to child nodes
flags |= NODE_WORLD_MATRIX_ACTUAL;
uint32 size = (uint32)childs.size();
for (uint32 c = 0; c < size; ++c)
{
childs[c]->InvalidateLocalTransform();
childs[c]->Update(timeElapsed);
}
}
else
{
uint32 size = (uint32)childs.size();
for (uint32 c = 0; c < size; ++c)
{
childs[c]->Update(timeElapsed);
}
}
//printf("- node: %s tr: %f %f %f\n", name.c_str(), localTransform.data[12], localTransform.data[13], localTransform.data[14]);
inUpdate = false;
if (!removedCache.empty())
{
for (std::deque<SceneNode*>::iterator t = removedCache.begin(); t != removedCache.end(); ++t)
{
RemoveNode(*t);
}
removedCache.clear();
}
}