bool LinePickVisitor::AddHitPoint(const Vector3& p, Vector3& offset)
{
// allocate a hit
PickHit* hit = AddHit ();
// our intersection point is the point itself
Vector3 intersection (p);
// transform values into world space
m_CurrentWorldTransform.TransformNormal(offset);
m_CurrentWorldTransform.TransformVertex(intersection);
// set vertex in world space
if (!HasFlags(PickFlags::IgnoreVertex))
{
hit->SetVertex(intersection, offset.Length());
}
// set the intersection in world space
if (!HasFlags(PickFlags::IgnoreIntersection))
{
hit->SetIntersection(intersection, offset.Length());
}
return true;
}
bool LinePickVisitor::PickTriangle(const Vector3& v0, const Vector3& v1, const Vector3& v2, const float err)
{
float32_t u = 0.f;
float32_t v = 0.f;
bool interior = true;
bool success = m_PickSpaceLine.IntersectsTriangle (v0, v1, v2, &u, &v);
Vector3 vertex;
Vector3 intersection;
float32_t distance = NumericLimits<float32_t>::Maximum;
if (!success)
{
interior = false;
float mu;
Vector3 offset;
if (m_PickSpaceLine.IntersectsSegment(v0, v1, err, &mu, &offset))
{
float len = offset.Length();
if (len < distance)
{
vertex = mu < 0.5f ? v0 : v1;
distance = len;
success = true;
}
}
if (m_PickSpaceLine.IntersectsSegment(v1, v2, err, &mu, &offset))
{
float len = offset.Length();
if (len < distance)
{
vertex = mu < 0.5f ? v1 : v2;
distance = len;
success = true;
}
}
if (m_PickSpaceLine.IntersectsSegment(v2, v0, err, &mu, &offset))
{
float len = offset.Length();
if (len < distance)
{
vertex = mu < 0.5f ? v2 : v0;
distance = len;
success = true;
}
}
}
if (success)
{
success = AddHitTriangle(v0, v1, v2, u, v, interior, vertex, intersection, distance);
}
return false;
}
void World::MoveCamera(float timeStep)
{
static float yaw_ = 0.0f;
static float pitch_ = 0.0f;
Input* input = context->GetSubsystem<Input>();
const float MOVE_SPEED = 20.0f;
const float MOUSE_SENSITIVITY = 0.1f;
IntVector2 mouseMove = input->GetMouseMove();
yaw_ += MOUSE_SENSITIVITY * mouseMove.x_;
pitch_ += MOUSE_SENSITIVITY * mouseMove.y_;
pitch_ = Clamp(pitch_, -90.0f, 90.0f);
camera.node->SetRotation(Quaternion(pitch_, yaw_, 0.0f));
Quaternion camRotation = camera.node->GetWorldRotation();
camRotation.z_ = 0.0f;
Vector3 dir = Vector3::ZERO;
if (input->GetKeyDown('W'))
dir += Vector3::FORWARD;
if (input->GetKeyDown('S'))
dir += Vector3::BACK;
if (input->GetKeyDown('A'))
dir += Vector3::LEFT;
if (input->GetKeyDown('D'))
dir += Vector3::RIGHT;
if (input->GetKeyDown(KEY_SPACE))
{
dir += Vector3::UP;
}
if (dir.Length() > 0)
{
Vector3 curVel = player.body->GetLinearVelocity();
if (curVel.Length() < 5.0f)
{
float multipler = 1.0f;
//if (input->GetQualifierDown(KEY_SHIFT))
if (input->GetKeyDown(KEY_LSHIFT))
{
multipler = 2.0f;
}
player.body->SetLinearVelocity(multipler * 200.0f * (camRotation * dir) * timeStep);
}
}
}
void CreateTool::CalculateInstanceRadiusAndBounds( float32_t& instanceRadius, AlignedBox& bounds )
{
Editor::HierarchyNode* node = Reflect::SafeCast<Editor::HierarchyNode>( m_Instance );
bounds = node->GetObjectBounds();
Vector3 boundVector = bounds.maximum - bounds.minimum;
Vector3 out = boundVector * Editor::OutVector;
Vector3 side = boundVector * Editor::SideVector;
instanceRadius = MAX( out.Length(), side.Length() ) / 2.0f;
}
// 動くオブジェクトの反射
bool Collision::GetReflect( iexMesh* org, Vector3& pos, Vector3& vec, float rate )
{
// オブジェクトの逆行列を算出
org->Update();
Matrix mat = org->TransMatrix;
Matrix invMat; // 逆行列
D3DXMatrixInverse( &invMat, null, &mat );
// 逆行列でレイをローカル化
Vector3 invVec;
invVec.x = invMat._11 * vec.x + invMat._21 * vec.y + invMat._31 * vec.z;
invVec.y = invMat._12 * vec.x + invMat._22 * vec.y + invMat._32 * vec.z;
invVec.z = invMat._13 * vec.x + invMat._23 * vec.y + invMat._33 * vec.z;
Vector3 invPos;
invPos.x = invMat._11 * pos.x + invMat._21 * pos.y + invMat._31 * pos.z + invMat._41;
invPos.y = invMat._12 * pos.x + invMat._22 * pos.y + invMat._32 * pos.z + invMat._42;
invPos.z = invMat._13 * pos.x + invMat._23 * pos.y + invMat._33 * pos.z + invMat._43;
Vector3 v = invVec;
Vector3 p = invPos;
Vector3 out;
float d = 100.0f;
if ( org->RayPick( &out, &p, &v, &d ) >= 0 )
{
Vector3 vv = out - p;
float dd = vv.Length();
float dm = invVec.Length();
if ( dd < dm ){
v.Normalize(); // 法線算出
float dot = Vector3Dot( -invVec, v ); // 法線方向に射影
invVec = v*dot*2.0f - ( -invVec );
Vector3 p;
p.x = mat._11 * out.x + mat._21 * out.y + mat._31 * out.z + mat._41;
p.y = mat._12 * out.x + mat._22 * out.y + mat._32 * out.z + mat._42;
p.z = mat._13 * out.x + mat._23 * out.y + mat._33 * out.z + mat._43;
pos = p;
Vector3 v;
v.x = mat._11 * invVec.x + mat._21 * invVec.y + mat._31 * invVec.z;
v.y = mat._12 * invVec.x + mat._22 * invVec.y + mat._32 * invVec.z;
v.z = mat._13 * invVec.x + mat._23 * invVec.y + mat._33 * invVec.z;
vec = v*rate;
return true;
}
}
return false;
}
/*!
* Evaluates curvature at (x,y,z) through discrete finite difference scheme.
*/
float Implicit::GetCurvature(float x, float y, float z) const
{
const float& d = mDelta;
float r2d = 1.0f/(2.0f*d);
Vector3<float> gradient = -GetGradient(x,y,z);
Vector4<float> ex_gradient(gradient[0],gradient[1],gradient[2],1.0f);
Vector3<float> gradientsP[3] = { -GetGradient(x + d, y, z) , -GetGradient(x, y + d, z) , -GetGradient(x, y, z + d) };
Vector3<float> gradientsN[3] = { -GetGradient(x - d, y, z) , -GetGradient(x, y - d, z) , -GetGradient(x, y, z - d) };
float hessian[4][4] = {
{ ( gradientsP[0][0] - gradientsN[0][0] )*r2d, ( gradientsP[1][0] - gradientsN[1][0] )*r2d, ( gradientsP[2][0] - gradientsN[2][0] )*r2d , 0.0f},
{ ( gradientsP[0][1] - gradientsN[0][1] )*r2d, ( gradientsP[1][1] - gradientsN[1][1] )*r2d, ( gradientsP[2][1] - gradientsN[2][1] )*r2d , 0.0f},
{ ( gradientsP[0][2] - gradientsN[0][2] )*r2d, ( gradientsP[1][2] - gradientsN[1][2] )*r2d, ( gradientsP[2][2] - gradientsN[2][2] )*r2d , 0.0f},
{ 0.0f, 0.0f, 0.0f, 1.0f}
};
float hTrace = hessian[0][0] + hessian[1][1] + hessian[2][2];
//the - 1 is just because we have a 4d vector so the dot product has a 1 at the end we need to get rid of
//3x3 matrix is all we need here really
float hMult = ex_gradient*(Matrix4x4<float>(hessian)*ex_gradient) - 1.0f;
float gradLength = gradient.Length();
float gradLengthSq = gradient*gradient;
float curv = (hMult - gradLengthSq*hTrace)/(2.0f* gradLengthSq*gradLength);
return curv;
}
bool RoboCat::MoveToLocation( float inDeltaTime, const Vector3& inLocation )
{
bool finishedMove = false;
Vector3 toMoveVec = inLocation - GetLocation();
float distToTarget = toMoveVec.Length();
toMoveVec.Normalize2D();
if( distToTarget > 0.1f )
{
if ( distToTarget > ( kMoveSpeed * inDeltaTime ) )
{
SetLocation( GetLocation() + toMoveVec * inDeltaTime * kMoveSpeed );
}
else
{
//we're basically almost there, so set it to move location
SetLocation( inLocation );
finishedMove = true;
}
}
else
{
//since we're close, stop moving towards the target
finishedMove = true;
}
return finishedMove;
}
void Quaternion::FromRotationTo(const Vector3& start, const Vector3& end)
{
Vector3 normStart = start.Normalized();
Vector3 normEnd = end.Normalized();
float d = normStart.DotProduct(normEnd);
if (d > -1.0f + M_EPSILON)
{
Vector3 c = normStart.CrossProduct(normEnd);
float s = sqrtf((1.0f + d) * 2.0f);
float invS = 1.0f / s;
x_ = c.x_ * invS;
y_ = c.y_ * invS;
z_ = c.z_ * invS;
w_ = 0.5f * s;
}
else
{
Vector3 axis = Vector3::RIGHT.CrossProduct(normStart);
if (axis.Length() < M_EPSILON)
axis = Vector3::UP.CrossProduct(normStart);
FromAngleAxis(180.f, axis);
}
}
void rms::ComputeAlignAxisMatrix( const Vector3<Real> & vInitial,
const Vector3<Real> & vAlignWith, Matrix3<Real> & matrix )
{
// compute cosine of angle between vectors
Real axisDot = vAlignWith.Dot( vInitial );
// compute rotation axis
Vector3<Real> axisCross( vInitial.Cross( vAlignWith ) );
// apply rotation if necessary
if (axisCross.SquaredLength() > Wml::Math<Real>::EPSILON) {
// compute normalized axis and angle, then create rotation around axis
axisCross.Normalize();
Real fAngle = Math<Real>::ACos( axisDot / vAlignWith.Length() );
matrix.FromAxisAngle( axisCross, fAngle );
} else if (axisDot < (Real)0) {
// find some perpendicular vectors
Wml::Vector3<Real> vPerp1, vPerp2;
ComputePerpVectors( vInitial, vPerp1, vPerp2 );
matrix.FromAxisAngle( vPerp1, (Real)180 * Math<Real>::DEG_TO_RAD );
} else {
matrix = Matrix3<Real>::IDENTITY;
}
}
void SeekObject::Update3(){
auto PtrRigidbody = GetComponent<Rigidbody>();
//回転の更新
//Velocityの値で、回転を変更する
//これで進行方向を向くようになる
auto PtrTransform = GetComponent<Transform>();
Vector3 Velocity = PtrRigidbody->GetVelocity();
if (Velocity.Length() > 0.0f){
Vector3 Temp = Velocity;
Temp.Normalize();
float ToAngle = atan2(Temp.x, Temp.z);
Quaternion Qt;
Qt.RotationRollPitchYaw(0, ToAngle, 0);
Qt.Normalize();
//現在の回転を取得
Quaternion NowQt = PtrTransform->GetQuaternion();
//現在と目標を補間（10分の1）
NowQt.Slerp(NowQt, Qt, 0.1f);
PtrTransform->SetQuaternion(NowQt);
}
//常にyはm_BaseY
auto Pos = PtrTransform->GetPosition();
Pos.y = m_BaseY;
PtrTransform->SetPosition(Pos);
}
//----------------------------------------------------------------------------
Sphere Mgc::ContSphereOfAABB (int iQuantity, const Vector3* akPoint)
{
Vector3 kMin = akPoint[0], kMax = kMin;
for (int i = 1; i < iQuantity; i++)
{
if ( akPoint[i].x < kMin.x )
kMin.x = akPoint[i].x;
else if ( akPoint[i].x > kMax.x )
kMax.x = akPoint[i].x;
if ( akPoint[i].y < kMin.y )
kMin.y = akPoint[i].y;
else if ( akPoint[i].y > kMax.y )
kMax.y = akPoint[i].y;
if ( akPoint[i].z < kMin.z )
kMin.z = akPoint[i].z;
else if ( akPoint[i].z > kMax.z )
kMax.z = akPoint[i].z;
}
Sphere kSphere;
kSphere.Center() = 0.5f*(kMax + kMin);
Vector3 kHalfDiagonal = 0.5f*(kMax - kMin);
kSphere.Radius() = kHalfDiagonal.Length();
return kSphere;
}
bool PhysicsSystem::RaySphereCollision(const CollisionRay &r, const CollisionSphere &s, CollisionData *collisionData ){
Vector3 relativePos = r.m_pos - s.m_pos;
float relativeDist = relativePos.Length();
float dot = Vector3::Dot(relativePos, r.m_dir);
if(dot > s.m_radius) {
return false;
}
float d = (s.m_radius * s.m_radius) - ((relativeDist * relativeDist) - (dot * dot));
bool thisCollision = false;
if(d > 0) {
thisCollision = true;
//I guess this would be put in collision data.... use this later :D
//intersectpos = r.m_pos + (direction * (relativeDist - sqrt(d)));
}
return thisCollision;
}
void Spring::Update(float msec){
//F = -kx - c (n . vab)
//Calculate the world positions for the local positions
Vector3 posL = m_lhs->BuildTransform() * m_localPosL;
Vector3 posR = m_rhs->BuildTransform() * m_localPosR;
//Work out the direction between the two nodes
Vector3 forceDir = posR - posL;
//Calculate a value for the length between the two points - rest length.
//This is basically the amount of extension the spring has undertaken. (x)
float err = forceDir.Length() - m_length;
//We divide it by the rest length of the spring to get a normalized value for the
// extension of the spring
err /= m_length;
//Normalise the direction of the force
forceDir.Normalise();
Vector3& linVelL = m_lhs->GetLinearVelocity();
Vector3& linVelR = m_rhs->GetLinearVelocity();
//Calculate the force to be applied
Vector3 force = forceDir * (err * m_ks - Vector3::Dot(forceDir, (linVelL - linVelR) * m_kd));
m_lhs->ApplyForce(force*0.5f, Vector3(0,0,0));
m_rhs->ApplyForce(-force*0.5f, Vector3(0,0,0));
}
//----------------------------------------------------------------------------
void ConvexPolyhedron::Create (const vector<Vector3>& rakPoint,
const vector<int>& raiConnect)
{
assert( rakPoint.size() >= 4 && raiConnect.size() >= 4 );
int iVQuantity = rakPoint.size();
int iTQuantity = raiConnect.size()/3;
int iEQuantity = iVQuantity + iTQuantity - 2;
Reset(iVQuantity,iEQuantity,iTQuantity);
m_akPoint = rakPoint;
// Copy polyhedron points into vertex array. Compute centroid for use in
// making sure the triangles are counterclockwise oriented when viewed
// from the outside.
ComputeCentroid();
// get polyhedron edge and triangle information
for (int iT = 0, iIndex = 0; iT < iTQuantity; iT++)
{
// get vertex indices for triangle
int iV0 = raiConnect[iIndex++];
int iV1 = raiConnect[iIndex++];
int iV2 = raiConnect[iIndex++];
// make sure triangle is counterclockwise
Vector3& rkV0 = m_akPoint[iV0];
Vector3& rkV1 = m_akPoint[iV1];
Vector3& rkV2 = m_akPoint[iV2];
Vector3 kDiff = m_kCentroid - rkV0;
Vector3 kE1 = rkV1 - rkV0;
Vector3 kE2 = rkV2 - rkV0;
Vector3 kNormal = kE1.Cross(kE2);
Real fLength = kNormal.Length();
if ( fLength > 1e-06f )
{
kNormal /= fLength;
}
else
{
kNormal = kDiff;
kNormal.Unitize();
}
Real fDistance = kNormal.Dot(kDiff);
if ( fDistance < 0.0f )
{
// triangle is counterclockwise
Insert(iV0,iV1,iV2);
}
else
{
// triangle is clockwise
Insert(iV0,iV2,iV1);
}
}
UpdatePlanes();
}
void PlayerArrow::Update()
{
// アングル
Vector3 playerFront = Vector3(player->GetParameter().mat.GetFront() * Vector3(1, 0, 1)).Normalized();
if (playerFront.Length() != 0)
{
angle_ = Vector3::Inner(playerFront, -Vector3::Forward);
if (Vector3::Dot(playerFront, Vector3::Left) > 0.0f)
angle_ *= -1;
}
/* プレイヤーデータ */
// ポジション
Vector3 playerPos = player->GetParameter().mat.GetPosition();
Vector2 pos = Vector2(playerPos.x, -playerPos.z);
if (pos.Length() != 0.0f)
drawPos_ = MAP_DRAW_POSITION + pos.Normalized() * pos.Length() * RE_SIZE_SCALE;
else
drawPos_ = MAP_DRAW_POSITION;
/* 気流発生 */
isDash = player->ReturnTackleParameter().dashFlag;
if (isDash && isDash != prevDash)
world.UIAdd(UI_ID::FLOWROOT_UI, std::make_shared<FlowRoot>(world, player, &drawPos_, resPiece));
prevDash = isDash;
}
Mesh* Shop::ShopInteraction(double dt, Camera5 camera, Mesh** meshList)
{
float range = 20;
float offset = 0.5;
for (Vector3 temp = camera.view.Normalized(); temp.Length() <= range; temp += camera.view.Normalized())
{
if (meshList[SP2::GEO_STORE]->min != nullptr || meshList[SP2::GEO_STORE]->max != nullptr)
{
if ((temp.x + camera.position.x <= meshList[SP2::GEO_STORE]->max->x + meshList[SP2::GEO_STORE]->position.x + offset && temp.x + camera.position.x >= meshList[SP2::GEO_STORE]->min->x + meshList[SP2::GEO_STORE]->position.x - offset) //Check min and max for x
&& (temp.y + camera.position.y <= meshList[SP2::GEO_STORE]->max->y + meshList[SP2::GEO_STORE]->position.y + offset && temp.y + camera.position.y >= meshList[SP2::GEO_STORE]->min->y + meshList[SP2::GEO_STORE]->position.y - offset) //Check min and max for y
&& (temp.z + camera.position.z <= meshList[SP2::GEO_STORE]->max->z + meshList[SP2::GEO_STORE]->position.z + offset && temp.z + camera.position.z >= meshList[SP2::GEO_STORE]->min->z + meshList[SP2::GEO_STORE]->position.z - offset)) //Check min and max for z
{
if (meshList[SP2::GEO_STORE]->lookAtShop == false)
{
return nullptr;
break;
}
else
{
return meshList[SP2::GEO_STORE];
break;
}
}
}
}
return nullptr;
}
void SpaceScene::Tick(float deltaTime) {
camera->Update(deltaTime);
canvas->Update(deltaTime);
//Simulate gravity
for (std::unique_ptr<CelestialBody>& source : universe->bodies) {
//Apply the bodies force of gravity to other bodies
for (std::unique_ptr<CelestialBody>& target : universe->bodies) {
if (source == target) continue;
Vector3 displacement = (target->position - source->position);
float length = displacement.Length();
float r2 = length * length;
float totalMass = (source->data.mass + target->data.mass);
float magnitude = (universe->G * (totalMass / r2));
Vector3 force(-magnitude, 0, 0);
force = force * displacement.Normalize();
Vector3 velocity = (force / target->data.mass, 0, 0);
target->position += (velocity * deltaTime);
}
//Apply the bodies linear velocity
source->position += (source->linearVelocity * deltaTime);
}
}
void Door::OnUpdate()
{
// TODO: move this
static const float MoveSpeed = 50.0f;
auto gameObject = _gameObject.lock();
if (gameObject)
{
Vector3 toTarget = _targetPosition - gameObject->GetTransform().GetPosition();
if (toTarget.LengthSquared() > Math::Epsilon)
{
float dist = toTarget.Length();
Vector3 dir = Vector3::Normalize(toTarget);
float movementDist = MoveSpeed * gameObject->GetGameWorld()->GetTime().deltaTime;
if (movementDist > dist)
{
movementDist = dist;
}
gameObject->GetTransform().SetPosition(gameObject->GetTransform().GetPosition() + dir * movementDist);
}
}
// Always try to close the door, unless overridden by OnActivate()
_targetPosition = _closedPosition;
}
void OrbitCamera::Move(int dx, int dy)
{
using namespace math;
Matrix44 view = GetViewMatrix();
Vector3 e = GetEyePos();
Vector3 t = GetFocusPos();
Vector3 v = e - t;
Real l = v.Length();
Real offset_x = dx /120.0f * l * 0.2f;
Real offset_z = dy /120.0f * l * 0.2f;
Vector3 axis_x = GetAxisX();
Vector3 axis_z = GetAxisZ();
Vector3 axis_y = GetAxisY();
axis_z.y = 0;
axis_z.Normalize();
axis_x = -axis_x * offset_x * 0.5f;
axis_z = axis_z * offset_z * 0.5;
t += axis_x + axis_z;
e += axis_x + axis_z;;
SetEyePos(e);
SetFocusPos(t);
LookAtLH(e, t, axis_y);
}
bool CreateTool::ValidPosition( const AlignedBox& bounds, const Vector3& translation, float minDistance )
{
Editor::HierarchyNode* node = Reflect::SafeCast<Editor::HierarchyNode>( m_Instance );
FrustumPickVisitor frustumPick( m_Scene->GetViewport()->GetCamera(), Frustum( bounds ) );
m_Scene->Pick( &frustumPick );
V_PickHitSmartPtr::const_iterator resultsItr = frustumPick.GetHits().begin();
V_PickHitSmartPtr::const_iterator resultsEnd = frustumPick.GetHits().end();
for ( ; resultsItr != resultsEnd; ++resultsItr )
{
Editor::HierarchyNode* currentNode = Reflect::SafeCast<Editor::HierarchyNode>( (*resultsItr)->GetHitObject() );
if ( !currentNode->IsTransient() && ( s_PaintPreventAnyOverlap || node->IsSimilar( currentNode ) ) )
{
const Editor::Transform* transform = currentNode->GetTransform();
if ( !transform )
{
return false;
}
Vector3 position( transform->GetGlobalTransform().t.x, transform->GetGlobalTransform().t.y, transform->GetGlobalTransform().t.z );
Vector3 differenceVector = translation - position;
if ( differenceVector.Length() < minDistance )
{
return false;
}
}
}
return true;
}
void OrbitCamera::Zoom(int d)
{
using namespace math;
Vector3 e = GetEyePos();
Vector3 t = GetFocusPos();
Vector3 up = GetAxisY();
Vector3 v = e - t;
Real l = v.Length();
Real offset = d /120.0f * l * 0.2f;
l -= offset;
l = l <= 1.0f ? 1.0f : l;
v.Normalize();
v *= l;
e = t + v;
SetEyePos(e);
LookAtLH(e, t, up);
}
bool LinePickVisitor::AddHitSegment(const Vector3& p1,const Vector3& p2, float32_t mu, Vector3& offset)
{
// allocate a hit
PickHit* hit = AddHit ();
// the closest segment vertex
Vector3 vertex (mu < 0.5f ? p1 : p2);
// the actual intersection point
Vector3 intersection (p1 + (p2 - p1) * mu);
// transform values into world space
m_CurrentWorldTransform.TransformNormal(offset);
m_CurrentWorldTransform.TransformVertex(vertex);
m_CurrentWorldTransform.TransformVertex(intersection);
// set vertex in world space
if (!HasFlags(PickFlags::IgnoreVertex))
{
hit->SetVertex(vertex, (vertex - intersection).Length());
}
// set intersection in world space
if (!HasFlags(PickFlags::IgnoreIntersection))
{
hit->SetIntersection(intersection, offset.Length());
}
return true;
}
void Sphere::Merge(const Sphere& sphere)
{
if (radius_ < 0.0f)
{
center_ = sphere.center_;
radius_ = sphere.radius_;
return;
}
Vector3 offset = sphere.center_ - center_;
float dist = offset.Length();
// If sphere fits inside, do nothing
if (dist + sphere.radius_ < radius_)
return;
// If we fit inside the other sphere, become it
if (dist + radius_ < sphere.radius_)
{
center_ = sphere.center_;
radius_ = sphere.radius_;
}
else
{
Vector3 NormalizedOffset = offset / dist;
Vector3 min = center_ - radius_ * NormalizedOffset;
Vector3 max = sphere.center_ + sphere.radius_ * NormalizedOffset;
center_ = (min + max) * 0.5f;
radius_ = (max - center_).Length();
}
}
//http://www.geometrictools.com/LibFoundation/Distance/Distance.html
void Line::ProjectPointOnSegment( const Vector3& point, Vector3& projectedPoint ) const
{
Vector3 diff = point - m_Origin;
Vector3 direction = m_Point - m_Origin;
float32_t extent = direction.Length();
direction.Normalize();
float32_t segmentParam = direction.Dot(diff);
if ( -extent < segmentParam )
{
if ( segmentParam < extent )
{
projectedPoint = m_Origin + direction * segmentParam;
}
else
{
projectedPoint = m_Origin + direction * extent;
}
}
else
{
projectedPoint = m_Origin - direction * extent;
}
}
Real NaturalSpline3<Real>::GetSpeedKey (int key, Real t) const
{
Vector3<Real> velocity = mB[key] + t*(((Real)2)*mC[key] +
((Real)3)*t*mD[key]);
return velocity.Length();
}
void Explosion::UpdateExplosion(StringHash eventType, VariantMap& eventData)
{
float timeStep = eventData[Update::P_TIMESTEP].GetFloat();
rigidBody_->SetMass(Max(initialMass_*((0.1f - age_)/0.1f),0.01f));
light_->SetBrightness(Max(initialBrightness_*(0.32f - age_)/0.32f,0.0f));
if (rootNode_->IsEnabled() && masterControl_->world.scene->IsUpdateEnabled()) {
PODVector<RigidBody* > hitResults;
float radius = 2.0f * initialMass_ + age_ * 7.0f;
if (masterControl_->PhysicsSphereCast(hitResults,rootNode_->GetPosition(), radius, M_MAX_UNSIGNED)) {
for (int i = 0; i < hitResults.Size(); i++) {
Vector3 hitNodeWorldPos = hitResults[i]->GetNode()->GetWorldPosition();
if (!hitResults[i]->IsTrigger() && hitResults[i]->GetPosition().y_ > -0.1f) {
//positionDelta is used for force calculation
Vector3 positionDelta = hitNodeWorldPos - rootNode_->GetWorldPosition();
float distance = positionDelta.Length();
Vector3 force = positionDelta.Normalized() * Max(radius-distance, 0.0f)
* timeStep * 2342.0f * rigidBody_->GetMass();
hitResults[i]->ApplyForce(force);
//Deal damage
unsigned hitID = hitResults[i]->GetNode()->GetID();
float damage = rigidBody_->GetMass()*timeStep;
if(masterControl_->spawnMaster_->spires_.Keys().Contains(hitID)) {
masterControl_->spawnMaster_->spires_[hitID]->Hit(damage, 1);
}
else if(masterControl_->spawnMaster_->razors_.Keys().Contains(hitID)) {
masterControl_->spawnMaster_->razors_[hitID]->Hit(damage, 1);
}
}
}
}
}
}
//! 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;
}
}
cMatrix44 cCurveWithTime::GetTransformWithOffsetPos(Vector3 e_vOffsetPos,float e_fLength)
{
Vector3 l_vTarget = m_vCurrentPosition+(this->m_vCurrentDirection*e_fLength);
Vector3 l_vOffsetPos = e_vOffsetPos+m_vCurrentPosition-(e_vOffsetPos.Length()*m_vCurrentDirection);
return cMatrix44::LookAtMatrix(l_vOffsetPos,l_vTarget,Vector3::ZAxis);
//return cMatrix44::TranslationMatrix(l_vOffsetPos)*cMatrix44::ZAxisRotationMatrix(m_fCurrentPosToNextPointAngle);
}
bool CollisionDetector::CylinderSphereCollision(PhysicsNode& p0, PhysicsNode& p1, CollisionData* data) {
CollisionCylinder& cylinder = *(CollisionCylinder*)p0.GetCollisionVolume();
CollisionSphere& sphere = *(CollisionSphere*)p1.GetCollisionVolume();
Vector3 cylCenterVector = cylinder.GetEnd() - cylinder.GetStart();
Vector3 pos1 = p1.GetPosition() - cylinder.GetStart();
float distanceFactorFromEP1 = Vector3::Dot(p1.GetPosition() - cylinder.GetStart(), cylCenterVector) / Vector3::Dot(cylCenterVector, cylCenterVector);
if(distanceFactorFromEP1 < 0) distanceFactorFromEP1 = 0;// clamp to endpoints if neccesary
if(distanceFactorFromEP1 > 1) distanceFactorFromEP1 = 1;
Vector3 closestPoint = cylinder.GetStart() + (cylCenterVector * distanceFactorFromEP1);
Vector3 collisionVector = p1.GetPosition() - closestPoint;
float distance = collisionVector.Length();
Vector3 collisionNormal = collisionVector / distance;
if(distance < sphere.GetRadius() + cylinder.GetRadius())
{
//collision occurred. use collisionNormal to reflect sphere off cyl
float factor = Vector3::Dot(p1.GetLinearVelocity(), collisionNormal);
p1.SetLinearVelocity(p1.GetLinearVelocity() - (collisionNormal * factor * 0.8f));
const float distSq = LengthSq(collisionNormal);
//get the max distance before collision
const float sumRadius = sphere.GetRadius() + cylinder.GetRadius();
p1.SetPosition(p1.GetPosition() + Vector3(collisionNormal * (sumRadius - sqrtf(distSq))));
return true;
}
return false;
}
void rms::ComputeAlignZAxisMatrix( const Vector3<Real> & vAlignWith,
Matrix3<Real> & matrix, bool bInvert )
{
// compute cosine of angle between vectors
Real axisDot = vAlignWith.Dot( Vector3<Real>::UNIT_Z );
// compute rotation axis
Vector3<Real> axisCross( Vector3<Real>::UNIT_Z.Cross( vAlignWith ) );
Real fInverter = (bInvert) ? (Real)-1 : (Real)1;
// apply rotation if necessary
if (axisCross.SquaredLength() > Wml::Math<Real>::EPSILON) {
// compute normalized axis and angle, then create rotation around axis
axisCross.Normalize();
Real fAngle = Math<Real>::ACos( axisDot / vAlignWith.Length() );
matrix.FromAxisAngle( axisCross, fAngle * fInverter );
} else if (axisDot < (Real)0) {
matrix.FromAxisAngle( Vector3<Real>::UNIT_X, (Real)180 * Math<Real>::DEG_TO_RAD * fInverter );
} else {
matrix = Matrix3<Real>::IDENTITY;
}
}
