protected internal override IEnumerable<Star> Generate(Random random)
{
var density = Math.Max(0, random.NormallyDistributedSingle(_densityDeviation, _densityMean));
var countMax = Math.Max(0, (int)(_size * _size * _size * density));
if (countMax <= 0)
yield break;
var count = random.Next(countMax);
for (int i = 0; i < count; i++)
{
var pos = new Vector3(
random.NormallyDistributedSingle(_deviationX * _size, 0),
random.NormallyDistributedSingle(_deviationY * _size, 0),
random.NormallyDistributedSingle(_deviationZ * _size, 0)
);
var d = pos.Length() / _size;
var m = d * 2000 + (1 - d) * 15000;
var t = random.NormallyDistributedSingle(4000, m, 1000, 40000);
yield return new Star(
pos,
StarName.Generate(random),
t
);
}
}
public static float Angle(Vector3 a, Vector3 b)
{
float dotf = Vector3.Dot(a,b);
dotf = dotf/(a.Length()*b.Length());
float acos = (float)Math.Acos(dotf);
return acos*180.0f/3.1415f;
}
/// <summary>
/// Returns a Quaternion representing a rotation.
/// </summary>
/// <param name="axis">The axis to rotate around.</param>
/// <param name="angle">The angle to rotate by.</param>
/// <returns>A Quaternion representing the rotation.</returns>
public static Quaternion Rotation(Vector3 axis, double angle)
{
double real = Math.Cos(angle / 2.0);
Vector3 imaginary;
//normalize first
imaginary = axis.Multiply(1.0 / axis.Length());
imaginary = imaginary.Multiply(Math.Sin(angle / 2.0));
return new Quaternion(real, imaginary);
}
public Rectangle(Vector3 p, Vector3 _a, Vector3 _b, Vector3 _normal, string name)
: base(name)
{
p0 = p;
a = _a;
b = _b;
a_len_squared = a.LengthSquared();
b_len_squared = b.LengthSquared();
area = a.Length() * b.Length();
inv_area = 1.0f / area;
normal = _normal;
Shadows = false;
}
// in local space!
public override void GetClosestPoint(Vector3 point, ref Vector3 closestPoint, ref Vector3 normal, ref bool penetration, ref uint customData)
{
float dist = point.Length();
penetration = (dist < m_Radius);
//Handle case when point is in sphere origin
if (dist == 0)
{
normal.X = 1.0f;
normal.Y = 0.0f;
normal.Z = 0.0f;
closestPoint.X = m_Radius;
closestPoint.Y = 0.0f;
closestPoint.Z = 0.0f;
}
else
{
float d = dist;
normal = point/d;
closestPoint = point*(m_Radius / d);
}
customData = 0;
}
public override float GetDistanceToCenter(
Vector3 particlePosition, Vector3 particleVelocity,
out Vector3 alongAxis, out Vector3 aroundAxis, out Vector3 awayAxis)
{
// Along - following the main axis
alongAxis = mainAxis;
// Toward - tawards the main axis
awayAxis = particlePosition - fieldPosition;
awayAxis.Normalize();
// Around - around the main axis, following the right hand rule
aroundAxis = Vector3.Cross(alongAxis, awayAxis);
particlePosition -= fieldPosition;
inverseRotation.Rotate(ref particlePosition);
particlePosition /= fieldSize;
// Start of code for Sphere
var maxDist = particlePosition.Length() / radius;
// End of code for Sphere
return maxDist;
}
public override bool IsSafePath(Vector2[] path, int timeOffset = 0, int speed = -1, int delay = 0)
{
timeOffset += Game.Ping;
speed = speed == -1 ? (int)Player.Instance.MoveSpeed : speed;
if (path.Length <= 1) //lastissue = playerpos
{
//timeNeeded = -11;
if (!Player.Instance.IsRecalling())
{
return(IsSafe());
}
if (IsSafe())
{
return(true);
}
float timeLeft = (Player.Instance.GetBuff("recall").EndTime - Game.Time) * 1000;
return(GetAvailableTime(Player.Instance.Position.To2D()) > timeLeft);
}
//Skillshot with missile.
if (!string.IsNullOrEmpty(OwnSpellData.ObjectCreationName))
{
float r = Missile == null ? TimeDetected + OwnSpellData.Delay - Environment.TickCount : 0;
r -= timeOffset;
Vector3 pathDir = path[1].To3D() - path[0].To3D();
Vector3 skillDir = FixedEndPosition - CurrentPosition;
float a = path[0].X;
float w = path[0].Y;
float m = path[0].To3D().Z;
float v = CurrentPosition.X;
float k = CurrentPosition.Y;
float o = CurrentPosition.Z;
float b = pathDir.X;
float j = pathDir.Y;
float n = pathDir.Z;
float f = skillDir.X;
float l = skillDir.Y;
float p = skillDir.Z;
float c = speed;
float d = pathDir.Length();
float g = OwnSpellData.MissileSpeed;
float h = skillDir.Length();
/*nullstelle d/dt - min distance*/
double t = ((1000 * Math.Pow(d, 2) * g * h * l - 1000 * c * d * Math.Pow(h, 2) * j) * w + (1000 * b * c * d *
Math.Pow(h, 2) - 1000 * Math.Pow(d, 2) * f * g * h) * v + (c * d * g * h * n * p - Math.Pow(c, 2) *
Math.Pow(h, 2) * Math.Pow(n, 2) + c * d * g * h * j * l - Math.Pow(c, 2) * Math.Pow(h, 2) * Math.Pow(j, 2) -
Math.Pow(b, 2) * Math.Pow(c, 2) * Math.Pow(h, 2) + b * c * d * f * g * h) * r + (1000 * Math.Pow(d, 2) * g *
h * m - 1000 * Math.Pow(d, 2) * g * h * o) * p + 1000 * c * d * Math.Pow(h, 2) * n * o - 1000 * c * d *
Math.Pow(h, 2) * m * n - 1000 * Math.Pow(d, 2) * g * h * k * l + 1000 * c * d *
Math.Pow(h, 2) * j * k - 1000 * a * b * c * d * Math.Pow(h, 2) + 1000 * a * Math.Pow(d, 2) * f * g * h) /
(1000 * Math.Pow(d, 2) * Math.Pow(g, 2) * Math.Pow(p, 2) - 2000 * c * d * g * h * n * p +
1000 * Math.Pow(c, 2) * Math.Pow(h, 2) * Math.Pow(n, 2) + 1000 * Math.Pow(d, 2) * Math.Pow(g, 2) * Math.Pow(l, 2) -
2000 * c * d * g * h * j * l + 1000 * Math.Pow(c, 2) * Math.Pow(h, 2) * Math.Pow(j, 2) + 1000 * Math.Pow(b, 2) *
Math.Pow(c, 2) * Math.Pow(h, 2) - 2000 * b * c * d * f * g * h + 1000 * Math.Pow(d, 2) * Math.Pow(f, 2) *
Math.Pow(g, 2));
Vector3 myPosition = path[0].To3D() + (float)t * pathDir * c / pathDir.Length();
Vector3 misPosition = CurrentPosition + (float)t * skillDir * g / skillDir.Length();
bool valid = myPosition.Distance(Player.Instance) <= Player.Instance.Distance(path[1]) &&
misPosition.Distance(CurrentPosition) <= CurrentPosition.Distance(FixedEndPosition) && t >= 0;
if (!valid && t >= 0)
{
/*t out of skill range => set t to skillshot maxrange*/
if (misPosition.Distance(CurrentPosition) > CurrentPosition.Distance(FixedEndPosition))
{
t = CurrentPosition.Distance(FixedEndPosition) / OwnSpellData.MissileSpeed + r / 1000;
myPosition = path[0].To3D() + (float)t * pathDir * c / pathDir.Length();
//misPosition = FixedEndPosition;
return(ToPolygon().IsOutside(myPosition.To2D()));
}
/*t out of path range*/
if (myPosition.Distance(Player.Instance) > Player.Instance.Distance(path[1]))
{
return(ToPolygon().IsOutside(path[1]));
}
}
//timeNeeded = 1337;
return(!valid || ToPolygon().IsOutside(myPosition.To2D()));
}
var timeToExplode = TimeDetected + OwnSpellData.Delay - Environment.TickCount;
if (timeToExplode <= 0)
{
//timeNeeded = -9;
return(IsSafe());
}
var myPositionWhenExplodes = path.PositionAfter(timeToExplode, speed, delay + timeOffset);
bool b1 = IsSafe(myPositionWhenExplodes);
//timeNeeded = b ? -103 : Player.Instance.WalkingTime(allIntersections[0].Point) + timeOffset + delay - timeToExplode;
//timeNeeded = -12345;
return(b1);
}
public static float GetAngleBetweenVectors(Vector3 a, Vector3 b)
{
a.Normalize();
b.Normalize();
return (float)Math.Acos((Vector3.Dot(a,b)) / (a.Length() * b.Length()));
}
//
// solve unilateral raint (equality, direct method)
//
public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref Matrix world2A,
ref Matrix world2B,
ref Vector3 invInertiaADiag,
float invMassA,
ref Vector3 linvelA, ref Vector3 angvelA,
ref Vector3 rel_posA1,
ref Vector3 invInertiaBDiag,
float invMassB,
ref Vector3 linvelB, ref Vector3 angvelB,
ref Vector3 rel_posA2,
float depthA, ref Vector3 normalA,
ref Vector3 rel_posB1, ref Vector3 rel_posB2,
float depthB, ref Vector3 normalB,
ref float imp0, ref float imp1)
{
//(void)linvelA;
//(void)linvelB;
//(void)angvelB;
//(void)angvelA;
imp0 = 0f;
imp1 = 0f;
float len = System.Math.Abs(normalA.Length()) - 1f;
if (System.Math.Abs(len) >= MathUtil.SIMD_EPSILON)
return;
Debug.Assert(len < MathUtil.SIMD_EPSILON);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA,
ref invInertiaBDiag,invMassB);
// float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
// float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB);
float vel0 = Vector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1)));
float vel1 = Vector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1)));
// float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * m_tau * massTerm - vel0 * m_damping;
float dv1 = depthB * m_tau * massTerm - vel1 * m_damping;
// dC/dv * dv = -C
// jacobian * impulse = -error
//
//impulse = jacobianInverse * -error
// inverting 2x2 symmetric system (offdiagonal are equal!)
//
float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB);
float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag );
//imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet;
//imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet;
imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet;
//[a b] [d -c]
//[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc)
//[jA nD] * [imp0] = [dv0]
//[nD jB] [imp1] [dv1]
}
public void update(float elapsedTime)
{
Vector3 pos = GuiController.Instance.CurrentCamera.getPosition();
if (pos.X < -2000 || pos.Z < -2000 || pos.X > 0 || pos.Z > 0)
{
// reset
pos.X = -1000;
pos.Z = -1000;
GuiController.Instance.FpsCamera.setCamera(pos, pos + new Vector3(0, 0, 1));
}
//Activar animacion de caminando
int t = 0;
foreach (TgcSkeletalMesh m in enemigos)
{
Vector3 dir_escape = m.Position - pos;
dir_escape.Y = 0;
float dist = dir_escape.Length();
if (Math.Abs(dist) < 10)
{
// lo alcance, lo mato
bot_status[t] = 99;
}
else
{
switch (bot_status[t])
{
// escondido
case 0:
if (dist < 400)
{
// me escapo
bot_status[t] = 1;
}
break;
// escapando
case 1:
if (dist > 1000)
{
// me esconde
bot_status[t] = 0;
}
break;
// perseguir
case 2:
break;
}
}
switch (bot_status[t])
{
// escondido
case 0:
m.playAnimation("StandBy", true);
break;
// escapando
case 1:
dir_escape.Normalize();
m.rotateY((float)Math.Atan2(dir_escape.X, dir_escape.Z) - m.Rotation.Y + 3.1415f);
m.move(dir_escape * (vel_bot * elapsedTime));
float X = m.Position.X;
float Z = m.Position.Z;
if (X < -2000)
{
X = -1000;
}
if (X > 0)
{
X = -1000;
}
if (Z < -2000)
{
Z = -1000;
}
if (Z > 0)
{
Z = -1000;
}
m.Position = new Vector3(X, m.Position.Y, Z);
m.playAnimation("Run", true, 20);
break;
// persiguiendo
case 2:
dir_escape.Normalize();
if (Math.Abs(dir_escape.Z) > 0.01f)
{
m.rotateY((float)Math.Atan2(dir_escape.X, dir_escape.Z) - m.Rotation.Y);
m.move(dir_escape * (-60 * elapsedTime));
}
m.playAnimation("Run", true, 20);
break;
case 99:
m.rotateZ(3.1415f * 0.5f - m.Rotation.Z);
m.playAnimation("StandBy", true);
break;
}
m.updateAnimation();
++t;
}
Random rnd = new Random();
for (int i = 0; i < cant_balas; ++i)
{
timer_firing[i] -= elapsedTime;
if (timer_firing[i] < 0)
{
timer_firing[i] += total_timer_firing;
pos_bala[i] = pos + new Vector3(rnd.Next(-10, 10), rnd.Next(-10, 10), rnd.Next(-10, 10));
dir_bala[i] = GuiController.Instance.CurrentCamera.getLookAt() - pos;
dir_bala[i].Normalize();
}
else
{
pos_bala[i] = pos_bala[i] + dir_bala[i] * (vel_bala * elapsedTime);
}
}
}
/// <summary>
/// Calculate the sun's orbital position and its velocity.
/// </summary>
private void GenSunPos()
{
// Time in seconds since UTC to use to calculate sun position.
PosTime = CurrentTime;
if (m_SunFixed)
{
// SunFixedHour represents the "hour of day" we would like
// It's represented in 24hr time, with 0 hour being sun-rise
// Because our day length is probably not 24hrs {LL is 6} we need to do a bit of math
// Determine the current "day" from current time, so we can use "today"
// to determine Seasonal Tilt and what'not
// Integer math rounded is on purpose to drop fractional day, determines number
// of virtual days since Epoch
PosTime = CurrentTime / SecondsPerSunCycle;
// Since we want number of seconds since Epoch, multiply back up
PosTime *= SecondsPerSunCycle;
// Then offset by the current Fixed Sun Hour
// Fixed Sun Hour needs to be scaled to reflect the user configured Seconds Per Sun Cycle
PosTime += (ulong)((m_SunFixedHour / 24.0) * (ulong)SecondsPerSunCycle);
}
else
{
if (m_DayTimeSunHourScale != 0.5f)
{
ulong CurDaySeconds = CurrentTime % SecondsPerSunCycle;
double CurDayPercentage = (double)CurDaySeconds / SecondsPerSunCycle;
ulong DayLightSeconds = (ulong)(m_DayTimeSunHourScale * SecondsPerSunCycle);
ulong NightSeconds = SecondsPerSunCycle - DayLightSeconds;
PosTime = CurrentTime / SecondsPerSunCycle;
PosTime *= SecondsPerSunCycle;
if (CurDayPercentage < 0.5)
{
PosTime += (ulong)((CurDayPercentage / .5) * DayLightSeconds);
}
else
{
PosTime += DayLightSeconds;
PosTime += (ulong)(((CurDayPercentage - 0.5) / .5) * NightSeconds);
}
}
}
TotalDistanceTravelled = SunSpeed * PosTime; // distance measured in radians
OrbitalPosition = (float)(TotalDistanceTravelled % m_SunCycle); // position measured in radians
// TotalDistanceTravelled += HoursToRadians-(0.25*Math.PI)*Math.Cos(HoursToRadians)-OrbitalPosition;
// OrbitalPosition = (float) (TotalDistanceTravelled%SunCycle);
SeasonalOffset = SeasonSpeed * PosTime; // Present season determined as total radians travelled around season cycle
Tilt.W = (float)(m_AverageTilt + (m_SeasonalTilt * Math.Sin(SeasonalOffset))); // Calculate seasonal orbital N/S tilt
// m_log.Debug("[SUN] Total distance travelled = "+TotalDistanceTravelled+", present position = "+OrbitalPosition+".");
// m_log.Debug("[SUN] Total seasonal progress = "+SeasonalOffset+", present tilt = "+Tilt.W+".");
// The sun rotates about the Z axis
Position.X = (float)Math.Cos(-TotalDistanceTravelled);
Position.Y = (float)Math.Sin(-TotalDistanceTravelled);
Position.Z = 0;
// For interest we rotate it slightly about the X access.
// Celestial tilt is a value that ranges .025
Position *= Tilt;
// Finally we shift the axis so that more of the
// circle is above the horizon than below. This
// makes the nights shorter than the days.
Position = Vector3.Normalize(Position);
Position.Z = Position.Z + (float)HorizonShift;
Position = Vector3.Normalize(Position);
// m_log.Debug("[SUN] Position("+Position.X+","+Position.Y+","+Position.Z+")");
Velocity.X = 0;
Velocity.Y = 0;
Velocity.Z = (float)SunSpeed;
// Correct angular velocity to reflect the seasonal rotation
Magnitude = Position.Length();
if (m_SunFixed)
{
Velocity.X = 0;
Velocity.Y = 0;
Velocity.Z = 0;
}
else
{
Velocity = (Velocity * Tilt) * (1.0f / Magnitude);
}
// TODO: Decouple this, so we can get rid of Linden Hour info
// Update Region with new Sun Vector
// set estate settings for region access to sun position
if (receivedEstateToolsSunUpdate)
{
m_scene.RegionInfo.RegionSettings.SunVector = Position;
}
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List<IColisionable> obstaculos, int recursionDepth, ColisionInfo colisionInfo)
{
//Limitar recursividad
if (recursionDepth > 5)
{
return;
}
//Ver si la distancia a recorrer es para tener en cuenta
float distanceToTravelSq = movementVector.LengthSq();
if (distanceToTravelSq < EPSILON)
{
return;
}
//Posicion deseada
Vector3 originalSphereCenter = characterSphere.Center;
Vector3 nextSphereCenter = originalSphereCenter + movementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
float minCollisionDistSq = float.MaxValue;
Vector3 realMovementVector = movementVector;
TgcBoundingAxisAlignBox.Face collisionFace = null;
IColisionable collisionObstacle = null;
Vector3 nearestPolygonIntersectionPoint = Vector3.Empty;
foreach (IColisionable obstaculoBB in obstaculos)
{
//Obtener los polígonos que conforman las 6 caras del BoundingBox
TgcBoundingAxisAlignBox.Face[] bbFaces = obstaculoBB.GetTgcBoundingBox().computeFaces();
foreach (TgcBoundingAxisAlignBox.Face bbFace in bbFaces)
{
Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane);
TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector);
float brutePlaneDist;
Vector3 brutePlaneIntersectionPoint;
if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint))
{
continue;
}
float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq();
if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq)
{
continue;
}
//Obtener punto de colisión en el plano, según la normal del plano
float pDist;
Vector3 planeIntersectionPoint;
Vector3 sphereIntersectionPoint;
TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal);
bool embebbed = false;
bool collisionFound = false;
if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//Ver si el plano está embebido en la esfera
if (pDist <= characterSphere.Radius)
{
embebbed = true;
//TODO: REVISAR ESTO, caso embebido a analizar con más detalle
sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius;
}
//Esta fuera de la esfera
else
{
//Obtener punto de colisión del contorno de la esfera según la normal del plano
sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius);
//Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento
TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector);
if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//no hay colisión
continue;
}
}
//Ver si planeIntersectionPoint pertenece al polígono
Vector3 newMovementVector;
float newMoveDistSq;
Vector3 polygonIntersectionPoint;
if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace))
{
if (embebbed)
{
//TODO: REVISAR ESTO, nunca debería pasar
//throw new Exception("El polígono está dentro de la esfera");
}
polygonIntersectionPoint = planeIntersectionPoint;
collisionFound = true;
}
else
{
//Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara
polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint,
bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]);
//Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega
Vector3 reversePointSeg = polygonIntersectionPoint - movementVector;
if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint))
{
collisionFound = true;
}
}
if (collisionFound)
{
//Nuevo vector de movimiento acotado
newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint;
newMoveDistSq = newMovementVector.LengthSq();
//se colisiono con algo, lo agrego a la lista
colisionInfo.Add(obstaculoBB);
if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq)
{
minCollisionDistSq = newMoveDistSq;
realMovementVector = newMovementVector;
nearestPolygonIntersectionPoint = polygonIntersectionPoint;
collisionFace = bbFace;
collisionObstacle = obstaculoBB;
}
}
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (collisionFace == null)
{
//Avanzar hasta muy cerca
float movementLength = movementVector.Length();
movementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(movementVector);
return;
}
//Solo movernos si ya no estamos muy cerca
if (minCollisionDistSq >= EPSILON)
{
//Mover el BoundingSphere hasta casi la nueva posición real
float movementLength = realMovementVector.Length();
realMovementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(realMovementVector);
}
//Calcular plano de Sliding
Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint;
Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint;
slidePlaneNormal.Normalize();
Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Proyectamos el punto original de destino en el plano de sliding
TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal);
float slideT;
Vector3 slideDestinationPoint;
if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint))
{
//Nuevo vector de movimiento
Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint;
if (slideMovementVector.LengthSq() < EPSILON)
{
return;
}
//Recursividad para aplicar sliding
doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1, colisionInfo);
}
return;
}
/// <summary>
/// Creates a view matrix pointing in a direction with a given up vector.
/// </summary>
/// <param name="position">Position of the camera.</param>
/// <param name="forward">Forward direction of the camera.</param>
/// <param name="upVector">Up vector of the camera.</param>
/// <param name="viewMatrix">Look at matrix.</param>
public static void CreateView(ref Vector3 position, ref Vector3 forward, ref Vector3 upVector, out Matrix viewMatrix)
{
float length = forward.Length();
var z = forward / -length;
Vector3 x;
Vector3x.Cross(ref upVector, ref z, out x);
x = Vector3.Normalize(x);
Vector3 y;
Vector3x.Cross(ref z, ref x, out y);
viewMatrix.X = new Vector4(x.X, y.X, z.X, 0);
viewMatrix.Y = new Vector4(x.Y, y.Y, z.Y, 0);
viewMatrix.Z = new Vector4(x.Z, y.Z, z.Z, 0);
viewMatrix.W = new Vector4(
-Vector3.Dot(x, position),
-Vector3.Dot(y, position),
-Vector3.Dot(z, position), 1);
}
public static Vector3 UnitVector(Vector3 vector)
{
return(vector / vector.Length());
}
private void ProcessInput()
{
float deltaTime = (float)Game.UpdateTime.Elapsed.TotalSeconds;
translation = Vector3.Zero;
yaw = 0f;
pitch = 0f;
// Keyboard and Gamepad based movement
{
// Our base speed is: one unit per second:
// deltaTime contains the duration of the previous frame, let's say that in this update
// or frame it is equal to 1/60, that means that the previous update ran 1/60 of a second ago
// and the next will, in most cases, run in around 1/60 of a second from now. Knowing that,
// we can move 1/60 of a unit on this frame so that in around 60 frames(1 second)
// we will have travelled one whole unit in a second.
// If you don't use deltaTime your speed will be dependant on the amount of frames rendered
// on screen which often are inconsistent, meaning that if the player has performance issues,
// this entity will move around slower.
float speed = 1f * deltaTime;
Vector3 dir = Vector3.Zero;
if (Gamepad && Input.HasGamePad)
{
GamePadState padState = Input.DefaultGamePad.State;
// LeftThumb can be positive or negative on both axis (pushed to the right or to the left)
dir.Z += padState.LeftThumb.Y;
dir.X += padState.LeftThumb.X;
// Triggers are always positive, in this case using one to increase and the other to decrease
dir.Y -= padState.LeftTrigger;
dir.Y += padState.RightTrigger;
// Increase speed when pressing A, LeftShoulder or RightShoulder
// Here:does the enum flag 'Buttons' has one of the flag ('A','LeftShoulder' or 'RightShoulder') set
if ((padState.Buttons & (GamePadButton.A | GamePadButton.LeftShoulder | GamePadButton.RightShoulder)) != 0)
{
speed *= SpeedFactor;
}
}
if (Input.HasKeyboard)
{
// Move with keyboard
// Forward/Backward
if (Input.IsKeyDown(Keys.W) || Input.IsKeyDown(Keys.Up))
{
dir.Z += 1;
}
if (Input.IsKeyDown(Keys.S) || Input.IsKeyDown(Keys.Down))
{
dir.Z -= 1;
}
// Left/Right
if (Input.IsKeyDown(Keys.A) || Input.IsKeyDown(Keys.Left))
{
dir.X -= 1;
}
if (Input.IsKeyDown(Keys.D) || Input.IsKeyDown(Keys.Right))
{
dir.X += 1;
}
// Down/Up
if (Input.IsKeyDown(Keys.Q))
{
dir.Y -= 1;
}
if (Input.IsKeyDown(Keys.E))
{
dir.Y += 1;
}
// Increase speed when pressing shift
if (Input.IsKeyDown(Keys.LeftShift) || Input.IsKeyDown(Keys.RightShift))
{
speed *= SpeedFactor;
}
// If the player pushes down two or more buttons, the direction and ultimately the base speed
// will be greater than one (vector(1, 1) is farther away from zero than vector(0, 1)),
// normalizing the vector ensures that whichever direction the player chooses, that direction
// will always be at most one unit in length.
// We're keeping dir as is if isn't longer than one to retain sub unit movement:
// a stick not entirely pushed forward should make the entity move slower.
if (dir.Length() > 1f)
{
dir = Vector3.Normalize(dir);
}
}
// Finally, push all of that to the translation variable which will be used within UpdateTransform()
translation += dir * KeyboardMovementSpeed * speed;
}
// Keyboard and Gamepad based Rotation
{
// See Keyboard & Gamepad translation's deltaTime usage
float speed = 1f * deltaTime;
Vector2 rotation = Vector2.Zero;
if (Gamepad && Input.HasGamePad)
{
GamePadState padState = Input.DefaultGamePad.State;
rotation.X += padState.RightThumb.Y;
rotation.Y += -padState.RightThumb.X;
}
if (Input.HasKeyboard)
{
if (Input.IsKeyDown(Keys.NumPad2))
{
rotation.X += 1;
}
if (Input.IsKeyDown(Keys.NumPad8))
{
rotation.X -= 1;
}
if (Input.IsKeyDown(Keys.NumPad4))
{
rotation.Y += 1;
}
if (Input.IsKeyDown(Keys.NumPad6))
{
rotation.Y -= 1;
}
// See Keyboard & Gamepad translation's Normalize() usage
if (rotation.Length() > 1f)
{
rotation = Vector2.Normalize(rotation);
}
}
// Modulate by speed
rotation *= KeyboardRotationSpeed * speed;
// Finally, push all of that to pitch & yaw which are going to be used within UpdateTransform()
pitch += rotation.X;
yaw += rotation.Y;
}
// Mouse movement and gestures
{
// This type of input should not use delta time at all, they already are frame-rate independent.
// Lets say that you are going to move your finger/mouse for one second over 40 units, it doesn't matter
// the amount of frames occuring within that time frame, each frame will receive the right amount of delta:
// a quarter of a second -> 10 units, half a second -> 20 units, one second -> your 40 units.
if (Input.HasMouse)
{
// Rotate with mouse
if (Input.IsMouseButtonDown(MouseButton.Right))
{
Input.LockMousePosition();
Game.IsMouseVisible = false;
yaw -= Input.MouseDelta.X * MouseRotationSpeed.X;
pitch -= Input.MouseDelta.Y * MouseRotationSpeed.Y;
}
else
{
Input.UnlockMousePosition();
Game.IsMouseVisible = true;
}
}
// Handle gestures
foreach (var gestureEvent in Input.GestureEvents)
{
switch (gestureEvent.Type)
{
// Rotate by dragging
case GestureType.Drag:
var drag = (GestureEventDrag)gestureEvent;
var dragDistance = drag.DeltaTranslation;
yaw = -dragDistance.X * TouchRotationSpeed.X;
pitch = -dragDistance.Y * TouchRotationSpeed.Y;
break;
// Move along z-axis by scaling and in xy-plane by multi-touch dragging
case GestureType.Composite:
var composite = (GestureEventComposite)gestureEvent;
translation.X = -composite.DeltaTranslation.X * TouchMovementSpeed.X;
translation.Y = -composite.DeltaTranslation.Y * TouchMovementSpeed.Y;
translation.Z = (float)Math.Log(composite.DeltaScale + 1) * TouchMovementSpeed.Z;
break;
}
}
}
}
/// <summary>
/// Returns the acute angle between two vectors.
/// </summary>
public static float AngleBetween(this Vector3 first, Vector3 second)
{
return((float)Math.Acos(first.Dot(second) / (first.Length() * second.Length())));
}
public override async Task Execute()
{
var config = AppConfig.GetConfiguration <Config>("CameraScript");
flyingAround = config.FlyingAround;
//uiControl = Context.RenderContext.UIControl;
if (camera == null)
{
// Near plane and Far plane are swapped in order to increase float precision for far distance as near distance is already having
// lots of precisions by the 1/z perspective projection
//camera = new Camera(new R32G32B32_Float(200, 0, 200), new R32G32B32_Float(0, 0, 200), 1.2f, 1280.0f / 720.0f, 4000000.0f, 10.0f);
var zNear = 10.0f;
var zFar = 4000000.0f;
if (Context.RenderContext.IsZReverse)
{
var temp = zNear;
zNear = zFar;
zFar = temp;
}
camera = new Camera(new Vector3(200, 0, 200), new Vector3(0, 0, 200), 1.2f, RenderContext.Width, RenderContext.Height, (float)RenderContext.Width / RenderContext.Height, zNear, zFar);
camera.Mode = CameraMode.Free;
}
Speed = config.Speed;
//useful when we need to debug something from a specific point of view (we can first get the WorldToCamera value using a breakpoint then set it below)
/*Matrix mat = new Matrix();
* mat.M11 = 0.0267117824f;
* mat.M12 = -0.9257705f;
* mat.M13 = -0.377141267f;
* mat.M14 = 0.0f;
* mat.M21 = -0.9996432f;
* mat.M22 = -0.024737807f;
* mat.M23 = -0.0100777093f;
* mat.M24 = 0.0f;
* mat.M31 = 0.0f;
* mat.M32 = 0.377275884f;
* mat.M33 = -0.926100969f;
* mat.M34 = 0.0f;
* mat.M41 = 531.524963f;
* mat.M42 = 501.754761f;
* mat.M43 = 989.462646f;
* mat.M44 = 1.0f;
* camera.WorldToCamera = mat;*/
//uiControl.MouseWheel += OnUiControlOnMouseWheel;
var st = new Stopwatch();
st.Start();
var viewParameters = Context.RenderContext.RenderPassPlugins.OfType <MainPlugin>().FirstOrDefault().ViewParameters;
Context.Scheduler.Add(() => AutoswitchCamera(Context));
var lastTime = DateTime.UtcNow;
var pauseTime = false;
while (true)
{
await Scheduler.Current.NextFrame();
var mousePosition = Context.InputManager.MousePosition;
if (Context.InputManager.IsMouseButtonPressed(MouseButton.Right))
{
camera.Mode = CameraMode.Free;
isModifyingPosition = true;
pitch = 0;
yaw = 0;
roll = 0;
if (camera.Mode == CameraMode.Free)
{
fixedFreeMatrix = camera.WorldToCamera;
}
else
{
fixedPosition = camera.Position;
}
}
if (Context.InputManager.IsMouseButtonDown(MouseButton.Right))
{
var deltaX = mousePosition.X - previousX;
var deltaY = mousePosition.Y - previousY;
if (isModifyingPosition)
{
yaw += deltaX * Speed / 1000.0f;
pitch += deltaY * Speed / 1000.0f;
if (camera.Mode == CameraMode.Target)
{
camera.Position = (Vector3)Vector3.Transform(fixedPosition, Matrix.RotationX(pitch) * Matrix.RotationZ(yaw));
Console.WriteLine(camera.Position);
}
else
{
camera.WorldToCamera = Camera.YawPitchRoll(camera.Position, fixedFreeMatrix, yaw, -pitch, roll);
}
//uiControl.Text = "" + camera.Mode;
viewChanged = true;
}
else if (isModifyingFov)
{
camera.FieldOfView += deltaX / 128.0f;
camera.FieldOfView = Math.Max(Math.Min(camera.FieldOfView, (float)Math.PI * 0.9f), 0.01f);
viewChanged = true;
}
}
if (Context.InputManager.IsMouseButtonReleased(MouseButton.Right))
{
isModifyingFov = false;
isModifyingPosition = false;
if (camera.Mode == CameraMode.Free && flyingAround)
{
camera.Mode = CameraMode.Target;
}
}
previousX = mousePosition.X;
previousY = mousePosition.Y;
if (Context.InputManager.IsKeyDown(Keys.LeftAlt) && Context.InputManager.IsKeyPressed(Keys.Enter))
{
Context.RenderContext.GraphicsDevice.IsFullScreen = !Context.RenderContext.GraphicsDevice.IsFullScreen;
}
if (Context.InputManager.IsKeyPressed(Keys.F2))
{
pauseTime = !pauseTime;
}
var currentTime = DateTime.UtcNow;
if (!pauseTime)
{
Context.CurrentTime += currentTime - lastTime;
viewParameters.Set(GlobalKeys.Time, (float)Context.CurrentTime.TotalSeconds);
var timeStep = (float)(currentTime - lastTime).TotalMilliseconds;
viewParameters.Set(GlobalKeys.TimeStep, timeStep);
}
// Set the pause variable for the rendering
Context.RenderContext.IsPaused = pauseTime;
lastTime = currentTime;
if (Context.InputManager.IsKeyPressed(Keys.Space))
{
// Fetch camera list
var cameras = Context.EntityManager.Entities
.Where(x => x.ContainsKey(CameraComponent.Key))
.Select(x => x.Get(CameraComponent.Key)).ToArray();
// Go to next camera
// If no camera or unset, index will be 0, so it will go to first one
int index = Array.IndexOf(cameras, TrackingCamera) + 1;
TrackingCamera = (index < cameras.Length) ? cameras[index] : null;
clock.Restart();
//flyingAround = !flyingAround;
//if (flyingAround)
//{
// fixedPosition = camera.Position;
// clock.Restart();
//}
}
if (TrackingCamera != null)
{
Matrix projection, worldToCamera;
// Overwrite near/far plane with our camera, as they are not reliable
TrackingCamera.NearPlane = camera.NearClipPlane;
TrackingCamera.FarPlane = camera.FarClipPlane;
TrackingCamera.Calculate(out projection, out worldToCamera);
viewParameters.Set(TransformationKeys.View, worldToCamera);
viewParameters.Set(TransformationKeys.Projection, projection);
// TODO: tracking camera doesn't have proper near/far plane values. Use mouse camera near/far instead.
viewParameters.Set(CameraKeys.NearClipPlane, camera.NearClipPlane);
viewParameters.Set(CameraKeys.FarClipPlane, camera.FarClipPlane);
viewParameters.Set(CameraKeys.FieldOfView, TrackingCamera.VerticalFieldOfView);
// Console.WriteLine("FOV:{0}", trackingCamera.VerticalFieldOfView);
viewParameters.Set(CameraKeys.ViewSize, new Vector2(camera.Width, camera.Height));
viewParameters.Set(CameraKeys.Aspect, TrackingCamera.AspectRatio);
viewParameters.Set(CameraKeys.FocusDistance, TrackingCamera.FocusDistance);
}
else
{
bool moved = false;
var localPosition = new Vector3(0, 0, 0);
if (Context.InputManager.IsKeyDown(Keys.Left) || Context.InputManager.IsKeyDown(Keys.A))
{
localPosition.X -= 1.0f;
moved = true;
}
if (Context.InputManager.IsKeyDown(Keys.Right) || Context.InputManager.IsKeyDown(Keys.D))
{
localPosition.X += 1.0f;
moved = true;
}
if (Context.InputManager.IsKeyDown(Keys.Up) || Context.InputManager.IsKeyDown(Keys.W))
{
localPosition.Y -= 1.0f;
moved = true;
}
if (Context.InputManager.IsKeyDown(Keys.Down) || Context.InputManager.IsKeyDown(Keys.S))
{
localPosition.Y += 1.0f;
moved = true;
}
if (Context.InputManager.IsKeyDown(Keys.R))
{
roll += 0.1f;
}
if (Context.InputManager.IsKeyDown(Keys.T))
{
roll -= 0.1f;
}
var moveSpeedFactor = Context.InputManager.IsKeyDown(Keys.LeftShift) || Context.InputManager.IsKeyDown(Keys.RightShift) ? 0.1f : 1.0f;
localPosition.Normalize();
localPosition *= (float)clock.ElapsedTicks * 1000.0f * MoveSpeed * moveSpeedFactor / Stopwatch.Frequency;
localVelocity = localVelocity * 0.9f + localPosition * 0.1f;
if (localVelocity.Length() > MoveSpeed * 0.001f)
{
if (camera.Mode == CameraMode.Target)
{
camera.Position = camera.Position + localVelocity;
}
else
{
var destVector = ((Vector3)camera.WorldToCamera.Column3);
var leftRightVector = Vector3.Cross(destVector, Vector3.UnitZ);
leftRightVector.Normalize();
var newPosition = camera.Position - destVector * localVelocity.Y;
newPosition = newPosition - leftRightVector * localVelocity.X;
camera.Position = newPosition;
}
viewChanged = true;
}
if (flyingAround)
{
camera.Position = (Vector3)Vector3.Transform(camera.Position, Matrix.RotationZ(clock.ElapsedMilliseconds * Speed / 10000.0f));
viewChanged = true;
}
if (Context.InputManager.IsKeyPressed(Keys.F))
{
camera.FieldOfView -= 0.1f;
viewChanged = true;
}
if (Context.InputManager.IsKeyPressed(Keys.G))
{
camera.FieldOfView += 0.1f;
viewChanged = true;
}
//if (viewChanged)
{
viewParameters.Set(TransformationKeys.View, camera.WorldToCamera);
viewParameters.Set(TransformationKeys.Projection, camera.Projection);
viewParameters.Set(CameraKeys.NearClipPlane, camera.NearClipPlane);
viewParameters.Set(CameraKeys.FarClipPlane, camera.FarClipPlane);
viewParameters.Set(CameraKeys.FieldOfView, camera.FieldOfView);
//Console.WriteLine("FOV:{0}", camera.FieldOfView);
viewParameters.Set(CameraKeys.ViewSize, new Vector2(camera.Width, camera.Height));
viewParameters.Set(CameraKeys.Aspect, camera.Aspect);
viewParameters.Set(CameraKeys.FocusDistance, 0.0f);
//Console.WriteLine("Camera: {0}", camera);
}
if (Context.InputManager.IsKeyPressed(Keys.I))
{
var worldToCamera = camera.WorldToCamera;
var target = (Vector3)worldToCamera.Column3;
Console.WriteLine("camera.Position = new R32G32B32_Float({0}f,{1}f,{2}f); camera.Target = camera.Position + new R32G32B32_Float({3}f, {4}f, {5}f);", camera.Position.X, camera.Position.Y, camera.Position.Z, target.X, target.Y, target.Z);
}
viewChanged = false;
clock.Restart();
}
}
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
/// <param name="eSphere">Sphere de radio 1 pasada a Elipsoid space</param>
/// <param name="eMovementVector">Movimiento pasado a Elipsoid space</param>
/// <param name="eRadius">Radio de la elipsoide</param>
/// <param name="colliders">Objetos contra los cuales colisionar</param>
/// <param name="recursionDepth">Nivel de recursividad</param>
/// <param name="movementSphere">Esfera real que representa el movimiento abarcado</param>
/// <param name="slidingMinY">Minimo valor de normal Y de colision para hacer sliding</param>
/// <returns>Resultado de colision</returns>
public CollisionResult doCollideWithWorld(TgcBoundingSphere eSphere, Vector3 eMovementVector, Vector3 eRadius, List <Collider> colliders, int recursionDepth, TgcBoundingSphere movementSphere, float slidingMinY)
{
CollisionResult result = new CollisionResult();
result.collisionFound = false;
//Limitar recursividad
if (recursionDepth > 5)
{
return(result);
}
//Posicion deseada
Vector3 nextSphereCenter = eSphere.Center + eMovementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
Vector3 q;
float t;
Vector3 n;
float minT = float.MaxValue;
foreach (Collider collider in colliders)
{
//Colisionar Sphere en movimiento contra Collider (cada Collider resuelve la colision)
if (collider.intersectMovingElipsoid(eSphere, eMovementVector, eRadius, movementSphere, out t, out q, out n))
{
//Quedarse con el menor instante de colision
if (t < minT)
{
minT = t;
result.collisionFound = true;
result.collisionPoint = q;
result.collisionNormal = n;
result.collider = collider;
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (!result.collisionFound)
{
//Avanzar todo lo pedido
eSphere.moveCenter(eMovementVector);
result.realMovmentVector = eMovementVector;
result.collisionNormal = Vector3.Empty;
result.collisionPoint = Vector3.Empty;
result.collider = null;
return(result);
}
//Solo movernos si ya no estamos muy cerca
if (minT >= EPSILON)
{
//Restar un poco al instante de colision, para movernos hasta casi esa distancia
minT -= EPSILON;
result.realMovmentVector = eMovementVector * minT;
eSphere.moveCenter(result.realMovmentVector);
//Quitarle al punto de colision el EPSILON restado al movimiento, para no afectar al plano de sliding
Vector3 v = Vector3.Normalize(result.realMovmentVector);
result.collisionPoint -= v * EPSILON;
}
//Calcular plano de Sliding, como un plano tangete al punto de colision con la esfera, apuntando hacia el centro de la esfera
Vector3 slidePlaneOrigin = result.collisionPoint;
Vector3 slidePlaneNormal = eSphere.Center - result.collisionPoint;
slidePlaneNormal.Normalize();
Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Calcular vector de movimiento para sliding, proyectando el punto de destino original sobre el plano de sliding
float distance = TgcCollisionUtils.distPointPlane(nextSphereCenter, slidePlane);
Vector3 newDestinationPoint = nextSphereCenter - distance * slidePlaneNormal;
Vector3 slideMovementVector = newDestinationPoint - result.collisionPoint;
//No hacer recursividad si es muy pequeño
slideMovementVector.Scale(slideFactor);
if (slideMovementVector.Length() < EPSILON)
{
return(result);
}
//Ver si posee la suficiente pendiente en Y para hacer sliding
if (result.collisionNormal.Y <= slidingMinY)
{
//Recursividad para aplicar sliding
doCollideWithWorld(eSphere, slideMovementVector, eRadius, colliders, recursionDepth + 1, movementSphere, slidingMinY);
}
return(result);
}
/// <summary>
/// Computes a convex shape description for a TransformableShape.
/// </summary>
///<param name="vA">First local vertex in the triangle.</param>
///<param name="vB">Second local vertex in the triangle.</param>
///<param name="vC">Third local vertex in the triangle.</param>
///<param name="collisionMargin">Collision margin of the shape.</param>
/// <returns>Description required to define a convex shape.</returns>
public static ConvexShapeDescription ComputeDescription(Vector3 vA, Vector3 vB, Vector3 vC, Fix64 collisionMargin)
{
ConvexShapeDescription description;
// A triangle by itself technically has no volume, but shapes try to include the collision margin in the volume when feasible (e.g. BoxShape).
//Plus, it's convenient to have a nonzero volume for buoyancy.
var doubleArea = Vector3.Cross(vB - vA, vC - vA).Length();
description.EntityShapeVolume.Volume = doubleArea * collisionMargin;
//Compute the inertia tensor.
var v = new Matrix3x3(
vA.X, vA.Y, vA.Z,
vB.X, vB.Y, vB.Z,
vC.X, vC.Y, vC.Z);
var s = new Matrix3x3(
F64.C2, F64.C1, F64.C1,
F64.C1, F64.C2, F64.C1,
F64.C1, F64.C1, F64.C2);
Matrix3x3.MultiplyTransposed(ref v, ref s, out description.EntityShapeVolume.VolumeDistribution);
Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, ref v, out description.EntityShapeVolume.VolumeDistribution);
var scaling = doubleArea / F64.C24;
Matrix3x3.Multiply(ref description.EntityShapeVolume.VolumeDistribution, -scaling, out description.EntityShapeVolume.VolumeDistribution);
//The square-of-sum term is ignored since the parameters should already be localized (and so would sum to zero).
var sums = scaling * (vA.LengthSquared() + vB.LengthSquared() + vC.LengthSquared());
description.EntityShapeVolume.VolumeDistribution.M11 += sums;
description.EntityShapeVolume.VolumeDistribution.M22 += sums;
description.EntityShapeVolume.VolumeDistribution.M33 += sums;
description.MinimumRadius = collisionMargin;
description.MaximumRadius = collisionMargin + MathHelper.Max(vA.Length(), MathHelper.Max(vB.Length(), vC.Length()));
description.CollisionMargin = collisionMargin;
return description;
}
public override void SolveConstraint(float timeStep)
{
Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
Vector3 normal = new Vector3(0, 0, 0);
float tau = 0.3f;
float damping = 1f;
//linear part
if (!_angularOnly)
{
for (int i = 0; i < 3; i++)
{
if (i == 0)
{
normal = new Vector3(1, 0, 0);
}
else if (i == 1)
{
normal = new Vector3(0, 1, 0);
}
else
{
normal = new Vector3(0, 0, 1);
}
float jacDiagABInv = 1f / _jac[i].Diagonal;
Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
//positional error (zeroth order error)
float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
AppliedImpulse += impulse;
Vector3 impulse_vector = normal * impulse;
RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
}
}
//solve angular part
// get axes in world space
Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
Vector3 angVelA = RigidBodyA.AngularVelocity;
Vector3 angVelB = RigidBodyB.AngularVelocity;
Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
Vector3 velrelOrthog = angAOrthog - angBOrthog;
//solve angular velocity correction
float relaxation = 1f;
float len = velrelOrthog.Length();
if (len > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(velrelOrthog);
float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
// scale for mass and relaxation
velrelOrthog *= (1f / denom) * 0.9f;
}
//solve angular positional correction
Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
float len2 = angularError.Length();
if (len2 > 0.00001f)
{
Vector3 normal2 = Vector3.Normalize(angularError);
float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
RigidBodyB.ComputeAngularImpulseDenominator(normal2);
angularError *= (1f / denom2) * relaxation;
}
RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
//apply motor
if (_enableAngularMotor)
{
//todo: add limits too
Vector3 angularLimit = Vector3.Zero;
Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
float projRelVel = Vector3.Dot(velrel, axisA);
float desiredMotorVel = _motorTargetVelocity;
float motorRelvel = desiredMotorVel - projRelVel;
float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
RigidBodyB.ComputeAngularImpulseDenominator(axisA);
float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
//todo: should clip against accumulated impulse
float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
Vector3 motorImp = clippedMotorImpulse * axisA;
RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
}
}
private Entity CheckEntityCollide(Vector3 position, Vector3 direction)
{
Ray2 ray = new Ray2
{
x = position,
d = Vector3.Normalize(direction)
};
var players = Level.GetSpawnedPlayers().OrderBy(player => Vector3.Distance(position, player.KnownPosition.ToVector3()));
foreach (var entity in players)
{
if (entity == Shooter) continue;
if (Intersect(entity.GetBoundingBox(), ray))
{
if (ray.tNear > direction.Length()) break;
Vector3 p = ray.x + new Vector3((float) ray.tNear)*ray.d;
KnownPosition = new PlayerLocation((float) p.X, (float) p.Y, (float) p.Z);
return entity;
}
}
var entities = Level.Entities.Values.OrderBy(entity => Vector3.Distance(position, entity.KnownPosition.ToVector3()));
foreach (Entity entity in entities)
{
if (entity == Shooter) continue;
if (entity == this) continue;
if (entity is Projectile) continue;
if (Intersect(entity.GetBoundingBox(), ray))
{
if (ray.tNear > direction.Length()) break;
Vector3 p = ray.x + new Vector3((float) ray.tNear)*ray.d;
KnownPosition = new PlayerLocation(p.X, p.Y, p.Z);
return entity;
}
}
return null;
}
private async void OnUpdate()
{
var movedLastTick = false;
while (this.IsActive)
{
if (!this.controller.IsConnected)
{
this.orbwalker.Value.OrbwalkingPoint = Vector3.Zero;
await Task.Delay(500);
continue;
}
State state;
if (!this.controller.GetState(out state))
{
this.orbwalker.Value.OrbwalkingPoint = Vector3.Zero;
await Task.Delay(250);
continue;
}
var orbwalkingModes = this.orbwalker.Value.CustomOrbwalkingModes.ToList();
if (orbwalkingModes.Any())
{
if (state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.DPadLeft))
{
this.customOrbwalkerIndex--;
if (this.customOrbwalkerIndex < 0)
{
this.customOrbwalkerIndex = orbwalkingModes.Count - 1;
}
this.SetActiveCustomOrbwalker(orbwalkingModes);
}
else if (state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.DPadRight))
{
this.customOrbwalkerIndex++;
if (this.customOrbwalkerIndex >= orbwalkingModes.Count)
{
this.customOrbwalkerIndex = 0;
}
this.SetActiveCustomOrbwalker(orbwalkingModes);
}
}
var tickRate = this.orbwalker.Value.Config.TickRate.Value;
if (Game.IsPaused || !this.owner.IsAlive)
{
await Task.Delay(tickRate);
continue;
}
var buttonX = state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.X);
var buttonY = state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.Y);
var buttonB = state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.B);
var buttonA = state.Gamepad.Buttons.HasFlag(GamepadButtonFlags.A);
var targetPosition = this.owner.NetworkPosition;
var leftStickPosition = new Vector3((float)state.Gamepad.LeftThumbX / short.MaxValue, (float)state.Gamepad.LeftThumbY / short.MaxValue, 0);
if (leftStickPosition.Length() > this.config.Deadzone)
{
leftStickPosition.Normalize();
targetPosition += leftStickPosition * Math.Max(this.orbwalker.Value.Settings.HoldRange + 50.0f, 200.0f);
}
else if (!buttonX && !buttonY && !buttonB && !buttonA)
{
if (movedLastTick)
{
this.owner.Stop();
movedLastTick = false;
}
await Task.Delay(tickRate);
continue;
}
if (buttonX)
{
this.orbwalker.Value.OrbwalkingPoint = targetPosition;
this.orbwalkerFarm.Execute();
movedLastTick = true;
await Task.Delay(tickRate);
continue;
}
if (buttonY)
{
this.orbwalker.Value.OrbwalkingPoint = targetPosition;
this.orbwalkerPush.Execute();
movedLastTick = true;
await Task.Delay(tickRate);
continue;
}
if (buttonB)
{
this.orbwalker.Value.OrbwalkingPoint = targetPosition;
this.orbwalkerSupport.Execute();
movedLastTick = true;
await Task.Delay(tickRate);
continue;
}
if (buttonA)
{
this.orbwalker.Value.OrbwalkingPoint = targetPosition;
this.orbwalkerCombo?.Execute();
movedLastTick = true;
await Task.Delay(tickRate);
continue;
}
this.orbwalker.Value.Move(targetPosition);
movedLastTick = true;
await Task.Delay(tickRate);
}
}
public override void _PhysicsProcess(float delta)
{
if (Input.IsActionJustPressed("ui_shift"))
{
velocity *= 3;
}
//if(this.posit)
/// <summary>
/// store our fall speed (gravity) because this will be destroyed by our motion computations...
/// </summary>
var fallSpeed = this.velocity.y;
//! use a shorter/simpler way to control character than shown in video
var motion = new Vector3()
{
x = Input.GetActionStrength("ui_right") - Input.GetActionStrength("ui_left"),
z = Input.GetActionStrength("ui_up") - Input.GetActionStrength("ui_down"),
};
if (this.Translation.y < -20)
{
this.Translation = new Vector3(0, 2, 0);
}
if (motion.Length() > 1)
{
//clamp to length 1 (otherwise pressing diaganol would give you speedup)
motion = motion.Normalized();
}
motion.z *= -1; //reverse z axis movement
motion *= SPEED;
//fake momentuym (speed up and down)
if (motion.Length() > 0)
{
this.velocity = this.velocity.LinearInterpolate(motion, ACCELERATION * delta);
}
else
{
this.velocity = this.velocity.LinearInterpolate(Vector3.Zero, ACCELERATION * delta);
}
//restore gravity
this.velocity.y = fallSpeed;
//apply jump
if (Input.IsActionPressed("ui_select") && canJump == true)
{
canJump = false;
this.velocity.y += 5; // Input.GetActionStrength("ui_select") * 10;
}
//dampen miniscule velocities
if (Mathf.Abs(this.velocity.x) < EPSILON)
{
this.velocity.x = 0;
}
if (Mathf.Abs(this.velocity.y) < EPSILON)
{
this.velocity.y = 0;
}
if (Mathf.Abs(this.velocity.z) < EPSILON)
{
this.velocity.z = 0;
}
//
if (this.velocity.Length() > MINVELOCITY)
{
var velForRotations = this.velocity;
velForRotations.y = 0;
if (velForRotations.LengthSquared() > EPSILON) //if we are moving along the gameplay plane, simulate rotation....
{
var cross = velForRotations.Cross(Vector3.Up);
var axis = cross.Normalized();
this.GetNode <MeshInstance>("MeshInstance").Rotate(axis, Mathf.Deg2Rad(-velForRotations.Length()));
}
this.velocity.y -= GRAVITY * delta;
var tryVel = this.velocity;
this.velocity = this.MoveAndSlide(this.velocity, Vector3.Up); //this.MoveAndCollide(motion * delta);
if (canJump == false && velocity.y < 2)
{
for (var i = 0; i < this.GetSlideCount(); i++)
{
var col = this.GetSlideCollision(i);
//GD.Print($"i={i} col={col.Normal}");
// GD.Print($"vel={this.velocity}, tryVel={tryVel}, col={col}");
if ((col.Normal.y / col.Normal.Length()) > EPSILON)
{
//GD.Print($"ALLOW JUMP! y=${col.Normal.y}, e=${EPSILON}");
canJump = true;
}
}
}
}
}
private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox)
{
#if !WINDOWS
boundingBox = new BoundingBox();
#endif
//Sample the local directions from the matrix, implicitly transposed.
var rightDirection = new Vector3(o.M11, o.M21, o.M31);
var upDirection = new Vector3(o.M12, o.M22, o.M32);
var backDirection = new Vector3(o.M13, o.M23, o.M33);
int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0;
float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue;
for (int i = 0; i < hullVertices.Count; i++)
{
float dotX, dotY, dotZ;
Vector3.Dot(ref rightDirection, ref hullVertices.Elements[i], out dotX);
Vector3.Dot(ref upDirection, ref hullVertices.Elements[i], out dotY);
Vector3.Dot(ref backDirection, ref hullVertices.Elements[i], out dotZ);
if (dotX < minX)
{
minX = dotX;
left = i;
}
if (dotX > maxX)
{
maxX = dotX;
right = i;
}
if (dotY < minY)
{
minY = dotY;
down = i;
}
if (dotY > maxY)
{
maxY = dotY;
up = i;
}
if (dotZ < minZ)
{
minZ = dotZ;
forward = i;
}
if (dotZ > maxZ)
{
maxZ = dotZ;
backward = i;
}
}
//Incorporate the collision margin.
Vector3.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection);
Vector3.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection);
Vector3.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection);
var rightElement = hullVertices.Elements[right];
var leftElement = hullVertices.Elements[left];
var upElement = hullVertices.Elements[up];
var downElement = hullVertices.Elements[down];
var backwardElement = hullVertices.Elements[backward];
var forwardElement = hullVertices.Elements[forward];
Vector3.Add(ref rightElement, ref rightDirection, out rightElement);
Vector3.Subtract(ref leftElement, ref rightDirection, out leftElement);
Vector3.Add(ref upElement, ref upDirection, out upElement);
Vector3.Subtract(ref downElement, ref upDirection, out downElement);
Vector3.Add(ref backwardElement, ref backDirection, out backwardElement);
Vector3.Subtract(ref forwardElement, ref backDirection, out forwardElement);
//Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
TransformLocalExtremePoints(ref rightElement, ref upElement, ref backwardElement, ref o, out boundingBox.Max);
TransformLocalExtremePoints(ref leftElement, ref downElement, ref forwardElement, ref o, out boundingBox.Min);
}
/// <summary>
/// フィギュアに含まれるnode treeを描画する。
/// </summary>
void DrawNodeTree()
{
TSOFile tso = selected_tso_file;
if (tso == null)
{
return;
}
Line line = new Line(device);
foreach (TSONode node in tso.nodes)
{
if (HiddenNode(node))
{
continue;
}
Vector3 p0 = GetNodePositionOnScreen(node);
p0.Z = 0.0f;
TSONode parent_node = node.parent;
if (parent_node != null)
{
if (HiddenNode(parent_node))
{
continue;
}
Vector3 p1 = GetNodePositionOnScreen(parent_node);
p1.Z = 0.0f;
Vector3 pd = p0 - p1;
float len = Vector3.Length(pd);
float scale = 4.0f / len;
Vector2 p3 = new Vector2(p1.X + pd.Y * scale, p1.Y - pd.X * scale);
Vector2 p4 = new Vector2(p1.X - pd.Y * scale, p1.Y + pd.X * scale);
Vector2[] vertices = new Vector2[3];
vertices[0] = new Vector2(p3.X, p3.Y);
vertices[1] = new Vector2(p0.X, p0.Y);
vertices[2] = new Vector2(p4.X, p4.Y);
line.Draw(vertices, TSONodeLineColor);
}
}
line.Dispose();
line = null;
Rectangle rect = new Rectangle(0, 16, 15, 15); //node circle
Vector3 rect_center = new Vector3(7, 7, 0);
sprite.Begin(SpriteFlags.None);
foreach (TSONode node in tso.nodes)
{
if (HiddenNode(node))
{
continue;
}
Vector3 p0 = GetNodePositionOnScreen(node);
p0.Z = 0.0f;
sprite.Draw(dot_texture, rect, rect_center, p0, Color.White);
}
sprite.End();
}
/// <summary>
/// Calculates the angle, measured in radians, between two vectors.
/// </summary>
public static float AngleBetween(Vector3 value1, Vector3 value2)
{
float a = Vector3.Dot(value1, value2) / (value1.Length() * value2.Length());
return (float)Math.Acos((double)MathHelper.Clamp(a, -1F, 1F));
}
private void CreateInnerSprings(Vector3 diff)
{
var diffSmall = (float)(diff.Length() / (3.0 * Math.Sqrt(3)));
var diffBig = (float)(diffSmall * Math.Sqrt(2));
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 3; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i, j, k + 1], diffSmall));
}
}
}
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 4; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i, j + 1, k], diffSmall));
}
}
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 4; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i + 1, j, k], diffSmall));
}
}
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 4; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i + 1, j + 1, k], diffBig));
springs.Add(new Spring(bezierPoints[i, j + 1, k], bezierPoints[i + 1, j, k], diffBig));
}
}
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < 3; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i + 1, j, k + 1], diffBig));
springs.Add(new Spring(bezierPoints[i, j, k + 1], bezierPoints[i + 1, j, k], diffBig));
}
}
}
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
springs.Add(new Spring(bezierPoints[i, j, k], bezierPoints[i, j + 1, k + 1], diffBig));
springs.Add(new Spring(bezierPoints[i, j, k + 1], bezierPoints[i, j + 1, k], diffBig));
}
}
}
}
/// <summary>
/// Analytic solutions useful fo hands or feet, all in local model space
/// </summary>
/// <param name="desiredEnd">in local model space</param>
/// <param name="firstBone"></param>
/// <param name="secondBone"></param>
/// <param name="finalTransform"></param>
/// <param name="finalBone"></param>
/// <param name="allowFinalBoneTranslation"></param>
/// <returns></returns>
public static bool SolveTwoJointsIk(ref Vector3 desiredEnd, MyCharacterBone firstBone, MyCharacterBone secondBone, MyCharacterBone endBone, ref Matrix finalTransform, Matrix WorldMatrix, MyCharacterBone finalBone = null, bool allowFinalBoneTranslation = true)
{
Matrix firstBoneAbsoluteTransform = firstBone.AbsoluteTransform;
Matrix secondBoneAbsoluteTransform = secondBone.AbsoluteTransform;
Matrix endBoneAbsoluteTransform = endBone.AbsoluteTransform;
Vector3 origin = firstBoneAbsoluteTransform.Translation;
Vector3 originToCurrentEnd = endBoneAbsoluteTransform.Translation - origin;
Vector3 originToDesiredEnd = desiredEnd - origin;
Vector3 firstBoneVector = secondBoneAbsoluteTransform.Translation - origin;
Vector3 secondBoneVector = originToCurrentEnd - firstBoneVector;
float firstBoneLength = firstBoneVector.Length();
float secondBoneLength = secondBoneVector.Length();
float originToDesiredEndLength = originToDesiredEnd.Length();
float originToCurrentEndLength = originToCurrentEnd.Length();
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
VRageRender.MyRenderProxy.DebugDrawSphere(Vector3.Transform(desiredEnd, WorldMatrix), 0.01f, Color.Red, 1, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToCurrentEnd, WorldMatrix), Color.Yellow, Color.Yellow, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + originToDesiredEnd, WorldMatrix), Color.Red, Color.Red, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + firstBoneVector, WorldMatrix), Color.Green, Color.Green, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + firstBoneVector, WorldMatrix), Vector3.Transform(origin + firstBoneVector + secondBoneVector, WorldMatrix), Color.Blue, Color.Blue, false);
}
// only two cases, the desired position is reachable or not
bool isDesiredEndReachable = firstBoneLength + secondBoneLength > originToDesiredEndLength;
// alpha = angle between the first bone and originToDesiredEnd vector
double finalAlpha = 0;
// beta = the angle between the first and second bone
double finalBeta = 0;
if (isDesiredEndReachable)
{ // we find proper angles
// cosine law c^2 = a^2 + b^2 - 2*a*b*cos(gamma)
// gamma = acos ( - (c^2 - a^2 - b^2) / (2*a*b) )
// alpha = angle between the first bone and originToDesiredEnd vector
double cosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToDesiredEndLength * originToDesiredEndLength) /
(2 * firstBoneLength * originToDesiredEndLength);
cosAlpha = MathHelper.Clamp(cosAlpha, -1, 1);
finalAlpha = Math.Acos(cosAlpha);
// beta = the angle between the first and second bone
double cosBeta = -(originToDesiredEndLength * originToDesiredEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cosBeta = MathHelper.Clamp(cosBeta, -1, 1);
finalBeta = Math.Acos(cosBeta);
// now get it to the root bone axis no
finalBeta = Math.PI - finalBeta;
}
// get the current angles
double cCosAlpha = -(secondBoneLength * secondBoneLength - firstBoneLength * firstBoneLength - originToCurrentEndLength * originToCurrentEndLength) /
(2 * firstBoneLength * originToCurrentEndLength);
cCosAlpha = MathHelper.Clamp(cCosAlpha, -1, 1);
double currentAlpha = Math.Acos(cCosAlpha);
double cCosBeta = -(originToCurrentEndLength * originToCurrentEndLength - firstBoneLength * firstBoneLength - secondBoneLength * secondBoneLength) /
(2 * firstBoneLength * secondBoneLength);
cCosBeta = MathHelper.Clamp(cCosBeta, -1, 1);
double currentBeta = Math.Acos(cCosBeta);
currentBeta = Math.PI - currentBeta;
Vector3 currentPlaneNormal = Vector3.Cross(firstBoneVector, originToCurrentEnd);
currentPlaneNormal.Normalize();
// we can now rotate the bones in current plane as if the desired end was on the currentEnd axis
float alphaDif = (float)(finalAlpha - currentAlpha);
float betaDif = (float)(finalBeta - currentBeta);
Matrix firstBoneRotation = Matrix.CreateFromAxisAngle(-currentPlaneNormal, alphaDif);
Matrix secondBoneRotation = Matrix.CreateFromAxisAngle(currentPlaneNormal, betaDif);
// now get the angle between original and final position plane normal
originToCurrentEnd.Normalize();
originToDesiredEnd.Normalize();
double dotProd = originToCurrentEnd.Dot(originToDesiredEnd);
dotProd = MathHelper.Clamp(dotProd, -1, 1);
double delta = Math.Acos(dotProd);
Vector3 planeRotationAxis = Vector3.Cross(originToCurrentEnd, originToDesiredEnd);
planeRotationAxis.Normalize();
// find the rotation matrices for bones in the original plane
Matrix planeRotation = Matrix.CreateFromAxisAngle(planeRotationAxis, (float)delta);
// compute the final rotations
firstBoneRotation = planeRotation * firstBoneRotation;
secondBoneRotation = secondBoneRotation * firstBoneRotation;
// draw the final positions if debug enabled
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_CHARACTER_IK_IKSOLVERS)
{
Vector3 rotatedFirst = Vector3.Transform(firstBoneVector, firstBoneRotation);
Vector3 rotatedSecond = Vector3.Transform(secondBoneVector, secondBoneRotation);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin, WorldMatrix), Vector3.Transform(origin + rotatedFirst, WorldMatrix), Color.Purple, Color.Purple, false);
VRageRender.MyRenderProxy.DebugDrawLine3D(Vector3.Transform(origin + rotatedFirst, WorldMatrix), Vector3.Transform(origin + rotatedFirst + rotatedSecond, WorldMatrix), Color.White, Color.White, false);
}
// Now we compute the final absolute transforms for the bones
Matrix firstBoneFinalAbsoluteTransform = firstBoneAbsoluteTransform * firstBoneRotation;
Matrix firstBoneParentAbsoluteTransform = firstBone.Parent.AbsoluteTransform;
Matrix localFirstBoneTransform = Matrix.Multiply(firstBoneFinalAbsoluteTransform, Matrix.Invert(firstBone.BindTransform * firstBoneParentAbsoluteTransform));
firstBone.Rotation = Quaternion.CreateFromRotationMatrix(localFirstBoneTransform);
firstBone.ComputeAbsoluteTransform();
Matrix secondBoneFinalAbsoluteTransform = secondBoneAbsoluteTransform * secondBoneRotation;
Matrix secondBoneParentAbsoluteTransform = secondBone.Parent.AbsoluteTransform;
Matrix localSecondBoneTransform = Matrix.Multiply(secondBoneFinalAbsoluteTransform, Matrix.Invert(secondBone.BindTransform * secondBoneParentAbsoluteTransform));
secondBone.Rotation = Quaternion.CreateFromRotationMatrix(localSecondBoneTransform);
secondBone.ComputeAbsoluteTransform();
// solve the last bone
if (finalBone != null && finalTransform.IsValid() && isDesiredEndReachable)
{
//MatrixD absoluteTransformEnd = finalBone.AbsoluteTransform * finalTransform; // this is our local final transform ( rotation)
// get the related transformation to original binding pose
MatrixD localTransformRelated;
if (allowFinalBoneTranslation)
{
localTransformRelated = finalTransform * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
}
else
{
localTransformRelated = finalTransform.GetOrientation() * MatrixD.Invert((MatrixD)finalBone.BindTransform * finalBone.Parent.AbsoluteTransform);
}
//localTransformRelated = Matrix.Normalize(localTransformRelated);
// from there get the rotation and translation
finalBone.Rotation = Quaternion.CreateFromRotationMatrix(Matrix.Normalize((Matrix)localTransformRelated.GetOrientation()));
if (allowFinalBoneTranslation)
{
finalBone.Translation = (Vector3)localTransformRelated.Translation;
}
finalBone.ComputeAbsoluteTransform();
}
return(isDesiredEndReachable);
}
/// <summary>
/// Batch a new border image draw to the draw list.
/// </summary>
/// <param name="texture">The texture to use during the draw</param>
/// <param name="worldMatrix">The world matrix of the element</param>
/// <param name="sourceRectangle">The rectangle indicating the source region of the texture to use</param>
/// <param name="elementSize">The size of the ui element</param>
/// <param name="borderSize">The size of the borders in the texture in pixels (left/right/top/bottom)</param>
/// <param name="color">The color to apply to the texture image.</param>
/// <param name="depthBias">The depth bias of the ui element</param>
/// <param name="imageOrientation">The rotation to apply on the image uv</param>
/// <param name="swizzle">Swizzle mode indicating the swizzle use when sampling the texture in the shader</param>
/// <param name="snapImage">Indicate if the image needs to be snapped or not</param>
public void DrawImage(Texture texture, ref Matrix worldMatrix, ref RectangleF sourceRectangle, ref Vector3 elementSize, ref Vector4 borderSize,
ref Color color, int depthBias, ImageOrientation imageOrientation = ImageOrientation.AsIs, SwizzleMode swizzle = SwizzleMode.None, bool snapImage = false)
{
// Check that texture is not null
if (texture == null)
{
throw new ArgumentNullException("texture");
}
// Skip items with null size
if (elementSize.Length() < MathUtil.ZeroTolerance)
{
return;
}
// Calculate the information needed to draw.
var drawInfo = new UIImageDrawInfo
{
Source =
{
X = sourceRectangle.X / texture.ViewWidth,
Y = sourceRectangle.Y / texture.ViewHeight,
Width = sourceRectangle.Width / texture.ViewWidth,
Height = sourceRectangle.Height / texture.ViewHeight
},
DepthBias = depthBias,
Color = color,
Swizzle = swizzle,
SnapImage = snapImage,
Primitive = borderSize == Vector4.Zero? PrimitiveType.Rectangle : PrimitiveType.BorderRectangle,
BorderSize = new Vector4(borderSize.X / sourceRectangle.Width, borderSize.Y / sourceRectangle.Width, borderSize.Z / sourceRectangle.Height, borderSize.W / sourceRectangle.Height),
};
var rotatedSize = imageOrientation == ImageOrientation.AsIs? elementSize: new Vector3(elementSize.Y, elementSize.X, 0);
drawInfo.VertexShift = new Vector4(borderSize.X / rotatedSize.X, 1f - borderSize.Y / rotatedSize.X, borderSize.Z / rotatedSize.Y, 1f - borderSize.W / rotatedSize.Y);
var matrix = worldMatrix;
matrix.M11 *= elementSize.X;
matrix.M12 *= elementSize.X;
matrix.M13 *= elementSize.X;
matrix.M21 *= elementSize.Y;
matrix.M22 *= elementSize.Y;
matrix.M23 *= elementSize.Y;
matrix.M31 *= elementSize.Z;
matrix.M32 *= elementSize.Z;
matrix.M33 *= elementSize.Z;
Matrix worldViewProjection;
Matrix.Multiply(ref matrix, ref viewProjectionMatrix, out worldViewProjection);
drawInfo.UnitXWorld = worldViewProjection.Row1;
drawInfo.UnitYWorld = worldViewProjection.Row2;
// rotate origin and unit axis if need.
var leftTopCorner = vector4LeftTop;
if (imageOrientation == ImageOrientation.Rotated90)
{
var unitX = drawInfo.UnitXWorld;
drawInfo.UnitXWorld = -drawInfo.UnitYWorld;
drawInfo.UnitYWorld = unitX;
leftTopCorner = new Vector4(-0.5f, 0.5f, 0, 1);
}
Vector4.Transform(ref leftTopCorner, ref worldViewProjection, out drawInfo.LeftTopCornerWorld);
var verticesPerElement = 4;
var indicesPerElement = 6;
if (drawInfo.Primitive == PrimitiveType.BorderRectangle)
{
verticesPerElement = 16;
indicesPerElement = 54;
}
var elementInfo = new ElementInfo(verticesPerElement, indicesPerElement, ref drawInfo, depthBias);
Draw(texture, ref elementInfo);
}
public static void IntegrateTransform(ref Matrix curTrans,ref Vector3 linvel,ref Vector3 angvel,float timeStep,ref Matrix predictedTransform)
{
predictedTransform = Matrix.Identity;
predictedTransform.Translation = (curTrans.Translation + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
Vector3 pos;
Quaternion predictedOrn;
Vector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
Vector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)System.Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
Quaternion dorn = new Quaternion(axis.X,axis.Y,axis.Z,(float)System.Math.Cos( fAngle*timeStep*.5f) );
Vector3 pos;
Quaternion rot;
Vector3 scale;
curTrans.Decompose(out scale, out rot, out pos);
Quaternion orn0 = rot;
Quaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
Matrix newMatrix = Matrix.CreateFromQuaternion(predictedOrn);
CopyMatrixRotation(ref newMatrix, ref predictedTransform);
}
private void UpdateOverriddenGyros()
{
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
{
Matrix invWorldRot = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
Vector3 velocityDiff = m_overrideTargetVelocity - localAngularVelocity;
if (velocityDiff == Vector3.Zero)
{
return;
}
UpdateOverrideAccelerationRampFrames(velocityDiff);
// acceleration = m/s * (1/s)
Vector3 desiredAcceleration = velocityDiff * (MyEngineConstants.UPDATE_STEPS_PER_SECOND / m_overrideAccelerationRampFrames.Value);
// CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
// I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
// Calculate the desired velocity correction torque
Vector3 desiredTorque = desiredAcceleration / invTensorVector;
// Calculate the available force for the correction by arbitrarily sum overridden gyros
// and the remaining force force of the controlled gyros
float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - ControlTorque.Length());
// Reduce the desired torque to the available force
Vector3 availableTorque = Vector3.ClampToSphere(desiredTorque, correctionForce);
Torque = ControlTorque * m_maxGyroForce + availableTorque;
Torque *= ResourceSink.SuppliedRatio;
const float TORQUE_SQ_LEN_TH = 0.0001f;
if (Torque.LengthSquared() < TORQUE_SQ_LEN_TH)
{
return;
}
m_grid.Physics.AddForce(MyPhysicsForceType.ADD_BODY_FORCE_AND_BODY_TORQUE, null, null, Torque * Sync.RelativeSimulationRatio);
}
}
/// <summary>
/// 渲染图标
/// </summary>
protected virtual void Render(DrawArgs drawArgs, Icon icon, Vector3 projectedPoint)
{
//判断当前Icon是否呗初始化了，比如，若重新定义了图标的位置，则它的isInitialized就是false
if (!icon.isInitialized)
{
icon.Initialize(drawArgs);
}
//判断当前图标是否在视地范围内，若不在，则不进行绘制
if (!drawArgs.WorldCamera.ViewFrustum.ContainsPoint(icon.Position))
{
return;
}
// 判断图标是否在最大，最小可见的范围内，若不在，则不进行绘制
double distanceToIcon = Vector3.Length(icon.Position - drawArgs.WorldCamera.Position);
if (distanceToIcon > icon.MaximumDisplayDistance)
{
return;
}
if (distanceToIcon < icon.MinimumDisplayDistance)
{
return;
}
//获得当前图层的纹理对象
IconTexture iconTexture = GetTexture(icon);
//判断是否是MouseOver对象
bool isMouseOver = icon == mouseOverIcon;
//若是MouseOver对象，则绘制Description里的内容
if (isMouseOver)
{
//若是MouseOver对象
isMouseOver = true;
//若当前图层可以操作，则设置当前的鼠标是Hand
if (icon.isSelectable)
{
DrawArgs.MouseCursor = CursorType.Hand;
}
////显示文字描述信息,暂时不需要
//string description = icon.Description;
//if(description==null)
// description = icon.ClickableActionURL;
////绘制文字信息
//if(description!=null)
//{
// //设置文字的绘制区域
// DrawTextFormat format = DrawTextFormat.NoClip | DrawTextFormat.WordBreak | DrawTextFormat.Bottom;
// int left = 10;
// if(World.Settings.showLayerManager)
// left += World.Settings.layerManagerWidth;
// Rectangle rect = Rectangle.FromLTRB(left, 10, drawArgs.screenWidth - 10, drawArgs.screenHeight - 10 );
// //绘制边框
// drawArgs.defaultDrawingFont.DrawText(
// m_sprite, description,
// rect,
// format, 0xb0 << 24 );
// rect.Offset(2,0);
// drawArgs.defaultDrawingFont.DrawText(
// m_sprite, description,
// rect,
// format, 0xb0 << 24 );
// rect.Offset(0,2);
// drawArgs.defaultDrawingFont.DrawText(
// m_sprite, description,
// rect,
// format, 0xb0 << 24 );
// rect.Offset(-2,0);
// drawArgs.defaultDrawingFont.DrawText(
// m_sprite, description,
// rect,
// format, 0xb0 << 24 );
// // 绘制文字信息
// rect.Offset(1,-1);
// drawArgs.defaultDrawingFont.DrawText(
// m_sprite, description,
// rect,
// format, descriptionColor );
//}
}
//获取颜色
int color = isMouseOver ? hotColor : normalColor;
if (iconTexture == null || isMouseOver || icon.NameAlwaysVisible)
{
// 绘制图标的名称
if (icon.Name != null)
{
// Render name field
const int labelWidth = 1000; // Dummy value needed for centering the text
if (iconTexture == null)
{
// Center over target as we have no bitmap
Rectangle rect = new Rectangle(
(int)projectedPoint.X - (labelWidth >> 1),
(int)(projectedPoint.Y - (drawArgs.iconNameFont.Description.Height >> 1)),
labelWidth,
drawArgs.screenHeight);
drawArgs.iconNameFont.DrawText(m_sprite, icon.Name, rect, DrawTextFormat.Center, color);
}
else
{
// Adjust text to make room for icon
int spacing = (int)(icon.Width * 0.3f);
if (spacing > 10)
{
spacing = 10;
}
int offsetForIcon = (icon.Width >> 1) + spacing;
Rectangle rect = new Rectangle(
(int)projectedPoint.X + offsetForIcon,
(int)(projectedPoint.Y - (drawArgs.iconNameFont.Description.Height >> 1)),
labelWidth,
drawArgs.screenHeight);
drawArgs.iconNameFont.DrawText(m_sprite, icon.Name, rect, DrawTextFormat.WordBreak, color);
}
}
}
//绘制图标
if (iconTexture != null)
{
// Render icon
float xscale = (float)icon.Width / iconTexture.Width;
float yscale = (float)icon.Height / iconTexture.Height;
m_sprite.Transform = Matrix.Scaling(xscale, yscale, 0);
if (icon.IsRotated)
{
m_sprite.Transform *= Matrix.RotationZ((float)icon.Rotation.Radians - (float)drawArgs.WorldCamera.Heading.Radians);
}
m_sprite.Transform *= Matrix.Translation(projectedPoint.X, projectedPoint.Y, 0);
m_sprite.Draw(iconTexture.Texture,
new Vector3(iconTexture.Width >> 1, iconTexture.Height >> 1, 0),
Vector3.Empty,
color);
// Reset transform to prepare for text rendering later
m_sprite.Transform = Matrix.Identity;
}
}
// NOTE: This method had problems with overridden gyros, so it's not used anymore in the normal
// code path. It is still used in the autopilot code path, though. For reference on the new code
// look at UpdateOverriddenGyros()
public Vector3 GetAngularVelocity(Vector3 control)
{
/*if (m_grid.GridControllers.IsControlledByLocalPlayer || (!m_grid.GridControllers.IsControlledByAnyPlayer && Sync.IsServer) || (false && Sync.IsServer))
* {*/
// Not checking whether engines are running, since ControlTorque should be 0 when
// engines are stopped (set by cockpit).
if (ResourceSink.SuppliedRatio > 0f && m_grid.Physics != null && m_grid.Physics.Enabled && !m_grid.Physics.RigidBody.IsFixed)
{
Matrix invWorldRot = m_grid.PositionComp.WorldMatrixInvScaled.GetOrientation();
Matrix worldRot = m_grid.WorldMatrix.GetOrientation();
Vector3 localAngularVelocity = Vector3.Transform(m_grid.Physics.AngularVelocity, ref invWorldRot);
// CH: CAUTION: Don't try to use InertiaTensor, although it might be more intuitive in some cases.
// I tried it and it's not an inverse of the InverseInertiaTensor! Only the InverseInertiaTensor seems to be correct!
var invTensor = m_grid.Physics.RigidBody.InverseInertiaTensor;
Vector3 invTensorVector = new Vector3(invTensor.M11, invTensor.M22, invTensor.M33);
var minInvTensor = invTensorVector.Min();
// Max rotation limiter
float divider = Math.Max(1, minInvTensor * INV_TENSOR_MAX_LIMIT);
// Calculate the velocity correction torque
Vector3 correctionTorque = Vector3.Zero;
Vector3 desiredAcceleration = (m_overrideTargetVelocity - localAngularVelocity) * VRage.Game.MyEngineConstants.UPDATE_STEPS_PER_SECOND;
// The correction is done by overridden gyros and by the remaining power of the controlled gyros
// This is not entirely physically correct, but it feels good
float correctionForce = m_maxOverrideForce + m_maxGyroForce * (1.0f - control.Length());
// This is to ensure that the correction is done uniformly in all axes
desiredAcceleration = desiredAcceleration * Vector3.Normalize(invTensorVector);
Vector3 desiredTorque = desiredAcceleration / invTensorVector;
float framesToDesiredVelocity = desiredTorque.Length() / correctionForce;
// If we are very close to the target velocity, just set it without applying the torque
const float minimalBypassVelocity = 0.005f * 0.005f;
if (framesToDesiredVelocity < 0.5f && m_overrideTargetVelocity.LengthSquared() < minimalBypassVelocity)
{
return(m_overrideTargetVelocity);
}
if (!Vector3.IsZero(desiredAcceleration, 0.0001f))
{
// The smoothing coefficient is here to avoid the slowdown stopping the ship abruptly, which doesn't look good
float smoothingCoeff = 1.0f - 0.8f / (float)Math.Exp(0.5f * framesToDesiredVelocity);
correctionTorque = Vector3.ClampToSphere(desiredTorque, correctionForce) * 0.95f * smoothingCoeff + desiredTorque * 0.05f * (1.0f - smoothingCoeff);
// A little black magic to make slowdown on large ships bigger
if (m_grid.GridSizeEnum == MyCubeSize.Large)
{
correctionTorque *= 2.0f;
}
}
Torque = (control * m_maxGyroForce + correctionTorque) / divider;
Torque *= ResourceSink.SuppliedRatio;
if (Torque.LengthSquared() > 0.0001f)
{
// Manually apply torque and use minimal component of inverted inertia tensor to make rotate same in all axes
var delta = Torque * new Vector3(minInvTensor) * VRage.Game.MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS;
var newAngularVelocity = localAngularVelocity + delta;
return(Vector3.Transform(newAngularVelocity, ref worldRot));
}
const float stoppingVelocitySq = 0.0003f * 0.0003f;
if (control == Vector3.Zero && m_overrideTargetVelocity == Vector3.Zero && m_grid.Physics.AngularVelocity != Vector3.Zero && m_grid.Physics.AngularVelocity.LengthSquared() < stoppingVelocitySq && m_grid.Physics.RigidBody.IsActive)
{
return(Vector3.Zero);
}
}
//}
if (m_grid.Physics != null)
{
return(m_grid.Physics.AngularVelocity);
}
else
{
return(Vector3.Zero);
}
}
/// <summary>
///
/// </summary>
/// <param name="position">Position in million km</param>
/// <returns></returns>
public static float SunIntensityFromPosition(Vector3 positionInMillKm)
{
float baseIntensity = MyBgrCubeConsts.EARTH_POSITION.Length();
float length = positionInMillKm.Length();
return MathHelper.Clamp(baseIntensity / (length), 0.3f, 5f);
}
private static float distance(Vector3 a, Vector3 b, Vector3 p)
{
return(cross(new Vector2(p.X - a.X, p.Y - a.Y), b).Length() / b.Length());
}
public override void OnTick()
{
base.OnTick();
if (KnownPosition.Y <= 0
|| (Velocity.Length() <= 0 && DespawnOnImpact)
|| (Velocity.Length() <= 0 && !DespawnOnImpact && Ttl <= 0))
{
DespawnEntity();
return;
}
Ttl--;
if (KnownPosition.Y <= 0 || Velocity.Length() <= 0) return;
Entity entityCollided = CheckEntityCollide(KnownPosition.ToVector3(), Velocity);
bool collided = false;
if (entityCollided != null)
{
double speed = Math.Sqrt(Velocity.X*Velocity.X + Velocity.Y*Velocity.Y + Velocity.Z*Velocity.Z);
double damage = Math.Ceiling(speed*Damage);
if (IsCritical)
{
damage += Level.Random.Next((int) (damage/2 + 2));
McpeAnimate animate = McpeAnimate.CreateObject();
animate.entityId = entityCollided.EntityId;
animate.actionId = 4;
Level.RelayBroadcast(animate);
}
Player player = entityCollided as Player;
if (player != null)
{
damage = player.CalculatePlayerDamage(player, damage);
}
entityCollided.HealthManager.TakeHit(this, (int) damage, DamageCause.Projectile);
entityCollided.HealthManager.LastDamageSource = Shooter;
collided = true;
}
else
{
var velocity2 = Velocity;
velocity2 *= (float) (1.0d - Drag);
velocity2 -= new Vector3(0, (float) Gravity, 0);
double distance = velocity2.Length();
velocity2 = Vector3.Normalize(velocity2)/2;
for (int i = 0; i < Math.Ceiling(distance)*2; i++)
{
PlayerLocation nextPos = (PlayerLocation) KnownPosition.Clone();
nextPos.X += (float) velocity2.X*i;
nextPos.Y += (float) velocity2.Y*i;
nextPos.Z += (float) velocity2.Z*i;
BlockCoordinates coord = new BlockCoordinates(nextPos);
Block block = Level.GetBlock(coord);
collided = block.Id != 0 && (block.GetBoundingBox()).Contains(nextPos.ToVector3());
if (collided)
{
SetIntersectLocation(block.GetBoundingBox(), KnownPosition);
break;
}
}
}
if (collided)
{
Velocity = Vector3.Zero;
}
else
{
KnownPosition.X += (float) Velocity.X;
KnownPosition.Y += (float) Velocity.Y;
KnownPosition.Z += (float) Velocity.Z;
Velocity *= (float) (1.0 - Drag);
Velocity -= new Vector3(0, (float) Gravity, 0);
KnownPosition.Yaw = (float) Velocity.GetYaw();
KnownPosition.Pitch = (float) Velocity.GetPitch();
}
// For debugging of flight-path
if (BroadcastMovement) BroadcastMoveAndMotion();
}
/// <summary>
/// Returns a position randomly distributed inside a sphere of unit radius
/// centered at the origin. Orientation will be random and length will range
/// between 0 and 1
/// </summary>
/// <returns></returns>
public static Vector3 RandomVectorInUnitRadiusSphere()
{
Vector3 v = new Vector3();
do
{
v.X = (RandomHelpers.Random() * 2) - 1;
v.Y = (RandomHelpers.Random() * 2) - 1;
v.Z = (RandomHelpers.Random() * 2) - 1;
}
while (v.Length() >= 1);
return v;
}
public override void SetSize(Vector3 size)
{
SetRadius(size.Length() / 2f);
}
private void CalculateSpeed()
{
// Get current time and change in time
double dt = 1000 / 60;
// Get delta position
pos = remote.GetPosition();
deltaPos = pos - oldPos;
oldPos = pos;
speed = deltaPos.Length() / dt * 1000;
// Get orientation vectors
orientation = remote.WorldMatrix;
if (inGravity) {
forwardVec = orientation.Forward;
rightVec = orientation.Right;
upVec = orientation.Up;
} else {
forwardVec = orientation.Up;
rightVec = orientation.Right;
upVec = orientation.Forward;
}
// Determine speed forward and sideways in m/s
if (inGravity) {
Vector3 gravityVec = -Vector3.Normalize(remote.GetNaturalGravity());
speedForward = Vector3.Dot(deltaPos, Vector3.Cross(gravityVec, rightVec)) / dt * 1000;
speedRight = Vector3.Dot(deltaPos, -Vector3.Cross(gravityVec, forwardVec)) / dt * 1000;
speedUp = Vector3.Dot(deltaPos, gravityVec) / dt * 1000;
} else {
speedForward = Vector3.Dot(deltaPos, upVec) / dt * 1000;
speedRight = Vector3.Dot(deltaPos, rightVec) / dt * 1000;
speedUp = Vector3.Dot(deltaPos, forwardVec) / dt * 1000;
}
}
public void CalcAngleInfo2(ref Matrix transA, ref Matrix transB, ref Matrix invInertiaWorldA, ref Matrix invInertiaWorldB)
{
m_swingCorrection = 0;
m_twistLimitSign = 0;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled)
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
Matrix trPose = Matrix.CreateFromQuaternion(m_qTarget);
Matrix trA = MathUtil.BulletMatrixMultiply(ref transA, ref m_rbAFrame);
Matrix trB = MathUtil.BulletMatrixMultiply(ref transB, ref m_rbBFrame);
Matrix trDeltaAB = MathUtil.BulletMatrixMultiply(trB, MathUtil.BulletMatrixMultiply(trPose, Matrix.Invert(trA)));
Quaternion qDeltaAB = Quaternion.CreateFromRotationMatrix(trDeltaAB);
Vector3 swingAxis = new Vector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
m_swingAxis = swingAxis;
m_swingAxis.Normalize();
m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
if (!MathUtil.FuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
// Not sure if these need order swapping as well?
Quaternion q1 = Quaternion.CreateFromRotationMatrix(transA);
Quaternion q2 = Quaternion.CreateFromRotationMatrix(m_rbAFrame);
Quaternion q3 = Quaternion.CreateFromRotationMatrix(transB);
Quaternion q4 = Quaternion.CreateFromRotationMatrix(m_rbBFrame);
Quaternion qA = Quaternion.CreateFromRotationMatrix(transA) * Quaternion.CreateFromRotationMatrix(m_rbAFrame);
Quaternion qB = Quaternion.CreateFromRotationMatrix(transB) * Quaternion.CreateFromRotationMatrix(m_rbBFrame);
Quaternion qAB = MathUtil.QuaternionInverse(qB) * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
Vector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
vConeNoTwist.Normalize();
Quaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
qABCone.Normalize();
Quaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
qABTwist.Normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
float swingAngle = 0f, swingLimit = 0f;
Vector3 swingAxis = Vector3.Zero;
ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1f;
if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
(swingLimit - (swingLimit * m_limitSoftness));
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
AdjustSwingAxisToUseEllipseNormal(ref swingAxis);
// Calculate necessary axis & factors
m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);
m_twistAxisA = Vector3.Zero;
m_kSwing = 1f /
(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
Vector3 ivA = Vector3.TransformNormal(MathUtil.MatrixColumn(this.m_rbAFrame, 0), transA);
Vector3 jvA = Vector3.TransformNormal(MathUtil.MatrixColumn(this.m_rbAFrame, 1), transA);
Vector3 kvA = Vector3.TransformNormal(MathUtil.MatrixColumn(this.m_rbAFrame, 2), transA);
Vector3 ivB = Vector3.TransformNormal(MathUtil.MatrixColumn(this.m_rbBFrame, 0), transB);
Vector3 target = Vector3.Zero;
float x = Vector3.Dot(ivB, ivA);
float y = Vector3.Dot(ivB, jvA);
float z = Vector3.Dot(ivB, kvA);
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -Vector3.Cross(ivB, ivA);
}
}
else
{
if (m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if (!(MathUtil.FuzzyZero(y)))
{
m_solveSwingLimit = true;
if (m_swingSpan2 >= m_fixThresh)
{
y = 0;
float span2 = (float)System.Math.Atan2(z, x);
if (span2 > m_swingSpan2)
{
x = (float)System.Math.Cos(m_swingSpan2);
z = (float)System.Math.Sin(m_swingSpan2);
}
else if (span2 < -m_swingSpan2)
{
x = (float)System.Math.Cos(m_swingSpan2);
z = -(float)System.Math.Sin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if (!MathUtil.FuzzyZero(z))
{
m_solveSwingLimit = true;
if (m_swingSpan1 >= m_fixThresh)
{
z = 0f;
float span1 = (float)System.Math.Atan2(y, x);
if (span1 > m_swingSpan1)
{
x = (float)System.Math.Cos(m_swingSpan1);
y = (float)System.Math.Sin(m_swingSpan1);
}
else if (span1 < -m_swingSpan1)
{
x = (float)System.Math.Cos(m_swingSpan1);
y = -(float)System.Math.Sin(m_swingSpan1);
}
}
}
}
target.X = x * ivA.X + y * jvA.X + z * kvA.X;
target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y;
target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z;
target.Normalize();
m_swingAxis = -Vector3.Cross(ivB, target);
m_swingCorrection = m_swingAxis.Length();
m_swingAxis.Normalize();
}
}
if (m_twistSpan >= 0f)
{
Vector3 twistAxis = Vector3.Zero;
ComputeTwistLimitInfo(ref qABTwist, ref m_twistAngle, ref twistAxis);
if (m_twistAngle > m_twistSpan * m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis);
m_kTwist = 1f /
(ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB));
}
if (m_solveSwingLimit)
{
m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis);
}
}
else
{
m_twistAngle = 0f;
}
}
}
public void Vector3LengthTest()
{
Vector2 a = new Vector2(1.0f, 2.0f);
float z = 3.0f;
Vector3 target = new Vector3(a, z);
float expected = (float)System.Math.Sqrt(14.0f);
float actual;
actual = target.Length();
Assert.IsTrue(MathHelper.Equal(expected, actual), "Vector3.Length did not return the expected value.");
}
void drawHighLights(Midi.Track midiTrack, TrackProps trackProps, float songPosP)
{
List <Midi.Note> noteList = midiTrack.Notes;
int hlNoteIndex = midiTrack.getLastNoteIndexAtTime((int)(Project.SongPosT - Project.PlaybackOffsetT));
if (hlNoteIndex < 0)
{
return;
}
Midi.Note note = noteList[hlNoteIndex], nextNote;
if (note.start > Project.Notes.SongLengthT) //only if audio ends before the notes end
{
return;
}
if (hlNoteIndex < noteList.Count - 1)
{
nextNote = noteList[hlNoteIndex + 1];
}
else
{
nextNote = note;
}
Vector3 noteStart = new Vector3(Project.getScreenPos(note.start, note.pitch), 0);
float noteEnd = Project.getScreenPos(note.stop, note.pitch).X;
Vector3 nextNoteStart = new Vector3(Project.getScreenPos(nextNote.start, nextNote.pitch), 0);
if (noteEnd > nextNoteStart.X && hlNoteIndex < noteList.Count - 1)
{
noteEnd = nextNoteStart.X;
}
if ((float)(nextNote.start - note.stop) > Qn_gapThreshold * Project.Notes.TicksPerBeat || note == nextNote)
{
if (nextNoteStart.X != noteStart.X)
{
nextNoteStart.Y = MathHelper.Lerp(noteStart.Y, nextNoteStart.Y, (float)(noteEnd - noteStart.X) / (nextNoteStart.X - noteStart.X));
}
nextNoteStart.X = noteEnd;
}
Vector3 hlPos = noteStart;
float noteLength = (noteEnd - noteStart.X);
float normPos = (songPosP - noteStart.X) / noteLength;
if ((bool)MovingHl)
{
float poweredNormPos = (float)Math.Pow(normPos, (double)HlMovementPow);
hlPos.X = noteStart.X + poweredNormPos * noteLength;
hlPos.Y = Project.getScreenPosY(trackProps.TrackView.Curve.Evaluate((float)Project.pixelsToTicks(hlPos.X)));
}
//Set common fx params---------------------
//For shrinking highlights
float shrinkPercent = normPos * 1.0001f;
if (!(bool)ShrinkingHl)
{
shrinkPercent = 0;
if ((bool)HlBorder)
{
shrinkPercent = 1;
}
}
fx.Parameters["ClipPercent"].SetValue(shrinkPercent);
float innerHlSize = VpHlSize * 0.5f * (1 - shrinkPercent);
fx.Parameters["InnerHlSize"].SetValue(innerHlSize);
//Vector4 hlColor;
//Texture2D hlTexture;
//getMaterial(trackProps, true, out hlColor, out hlTexture);
//Vector4 hlColor = fx.Parameters["HlColor"].GetValueVector4();
//fx.Parameters["HlColor"].SetValue(SongPanel.HSLA2RGBA(hlColor).ToVector4());
fx.Parameters["Border"].SetValue((bool)HlBorder);
//-----------------------------------------------
if (HlType == LineHlType.Arrow)
{
float arrowLength;
Vector3 arrowDir;
Vector3 arrowNormal;
if (!(bool)MovingHl) //Non-moving arrow
{
Vector3 nextNoteOffset = nextNoteStart - noteStart;
arrowLength = nextNoteOffset.Length();
arrowDir = nextNoteOffset;;
}
else //Moving arrow
{
float x1 = hlPos.X;
float y1 = hlPos.Y;
float x2 = x1 + 0.001f;
float pitch2 = trackProps.TrackView.Curve.Evaluate((float)Project.pixelsToTicks(x2));
float y2 = Project.getScreenPosY(pitch2);
arrowDir = new Vector3(x2 - x1, y2 - y1, 0);
arrowLength = VpHlSize * 1.25f; //Make arrow 25% longer than wide
}
arrowDir.Normalize();
arrowNormal = new Vector3(-arrowDir.Y, arrowDir.X, 0);
arrowNormal.Normalize();
float halfArrowWidth = VpHlSize * 0.5f;
lineHlVerts[0].pos = hlPos + arrowNormal * halfArrowWidth;
lineHlVerts[1].pos = hlPos - arrowNormal * halfArrowWidth;
lineHlVerts[2].pos = hlPos + arrowDir * arrowLength;
fx.Parameters["ArrowDir"].SetValue(arrowDir);
//lineFx.Parameters["ArrowLength"].SetValue(nextNoteOffsetLength);
fx.Parameters["ArrowStart"].SetValue(hlPos);
fx.Parameters["ArrowEnd"].SetValue(lineHlVerts[2].pos); //Is used to calc distance from the two "sides" of the triangle (not the bottom) since they share this point
Vector3 side1Tangent = lineHlVerts[2].pos - lineHlVerts[0].pos;
Vector3 side1Normal = new Vector3(-side1Tangent.Y, side1Tangent.X, 0);
side1Normal.Normalize();
fx.Parameters["Side1Normal"].SetValue(side1Normal);
Vector3 side2Tangent = lineHlVerts[2].pos - lineHlVerts[1].pos;
Vector3 side2Normal = new Vector3(-side2Tangent.Y, side2Tangent.X, 0);
side2Normal.Normalize();
fx.Parameters["Side2Normal"].SetValue(side2Normal);
//Calc shortest dist to incenter from border, ie. the inscribed circle's radius
float a = (lineHlVerts[0].pos - lineHlVerts[1].pos).Length();
float b = (lineHlVerts[0].pos - lineHlVerts[2].pos).Length();
float c = (lineHlVerts[1].pos - lineHlVerts[2].pos).Length();
float k = (a + b + c) / 2.0f;
float icRadius = (float)Math.Sqrt(k * (k - a) * (k - b) * (k - c)) / k;
fx.Parameters["DistToCenter"].SetValue(icRadius);
fx.CurrentTechnique = fx.Techniques["Arrow"];
fx.CurrentTechnique.Passes[0].Apply();
GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleList, lineHlVerts, 0, 1);
}
else if (HlType == LineHlType.Circle)
{
setHlCirclePos(hlPos);
fx.CurrentTechnique = fx.Techniques["Circle"];
fx.CurrentTechnique.Passes[0].Apply();
GraphicsDevice.DrawUserPrimitives(PrimitiveType.TriangleStrip, lineHlVerts, 0, 2);
}
}
public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
{
_simplexSolver.Reset();
// compute linear and angular velocity for this interval, to interpolate
Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
float boundingRadiusA = _convexA.GetAngularMotionDisc();
float boundingRadiusB = _convexB.GetAngularMotionDisc();
float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
angVelB.Length() * boundingRadiusB;
float radius = 0.001f;
float lambda = 0f;
Vector3 v = new Vector3(1f, 0f, 0f);
int maxIter = MaxIterations;
Vector3 n = new Vector3();
bool hasResult = false;
Vector3 c;
float lastLambda = lambda;
//float epsilon = 0.001f;
int numIter = 0;
//first solution, using GJK
Matrix identityTrans = Matrix.Identity;
SphereShape raySphere = new SphereShape(0f);
raySphere.Margin = 0f;
//result.drawCoordSystem(sphereTr);
PointCollector pointCollector1 = new PointCollector();
GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//we don't use margins during CCD
gjk.setIgnoreMargin(true);
input.TransformA = fromA;
input.TransformB = fromB;
DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
hasResult = pointCollector1.HasResult;
c = pointCollector1.PointInWorld;
if (hasResult)
{
float dist;
dist = pointCollector1.Distance;
n = pointCollector1.NormalOnBInWorld;
//not close enough
while (dist > radius)
{
numIter++;
if (numIter > maxIter)
{
return(false); //todo: report a failure
}
float dLambda = 0f;
//calculate safe moving fraction from distance / (linear+rotational velocity)
//float clippedDist = GEN_min(angularConservativeRadius,dist);
//float clippedDist = dist;
float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
lambda = lambda + dLambda;
if (lambda > 1f)
{
return(false);
}
if (lambda < 0f)
{
return(false);
}
//todo: next check with relative epsilon
if (lambda <= lastLambda)
{
break;
}
lastLambda = lambda;
//interpolate to next lambda
Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
result.DebugDraw(lambda);
PointCollector pointCollector = new PointCollector();
gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
input.TransformA = interpolatedTransA;
input.TransformB = interpolatedTransB;
// !!!!!!!!!!
r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
gjk.GetClosestPoints(input, r, null);
if (pointCollector.HasResult)
{
if (pointCollector.Distance < 0f)
{
//degenerate ?!
result.Fraction = lastLambda;
result.Normal = n;
return(true);
}
c = pointCollector.PointInWorld;
dist = pointCollector.Distance;
}
else
{
//??
return(false);
}
}
result.Fraction = lambda;
result.Normal = n;
return(true);
}
return(false);
}
/// <summary>
/// used by limitMaxDeviationAngle / limitMinDeviationAngle
/// </summary>
/// <param name="insideOrOutside"></param>
/// <param name="source"></param>
/// <param name="cosineOfConeAngle"></param>
/// <param name="basis"></param>
/// <returns></returns>
private static Vector3 LimitDeviationAngleUtility(bool insideOrOutside, Vector3 source, float cosineOfConeAngle, Vector3 basis)
{
// immediately return zero length input vectors
float sourceLength = source.Length();
if (sourceLength < float.Epsilon)
return source;
// measure the angular diviation of "source" from "basis"
Vector3 direction = source / sourceLength;
float cosineOfSourceAngle = Vector3.Dot(direction, basis);
// Simply return "source" if it already meets the angle criteria.
// (note: we hope this top "if" gets compiled out since the flag
// is a constant when the function is inlined into its caller)
if (insideOrOutside)
{
// source vector is already inside the cone, just return it
if (cosineOfSourceAngle >= cosineOfConeAngle)
return source;
}
else if (cosineOfSourceAngle <= cosineOfConeAngle)
return source;
// find the portion of "source" that is perpendicular to "basis"
Vector3 perp = PerpendicularComponent(source, basis);
if (perp == Vector3.Zero)
return Vector3.Zero;
// normalize that perpendicular
Vector3 unitPerp = Vector3.Normalize(perp);
// construct a new vector whose length equals the source vector,
// and lies on the intersection of a plane (formed the source and
// basis vectors) and a cone (whose axis is "basis" and whose
// angle corresponds to cosineOfConeAngle)
float perpDist = (float)Math.Sqrt(1 - (cosineOfConeAngle * cosineOfConeAngle));
Vector3 c0 = basis * cosineOfConeAngle;
Vector3 c1 = unitPerp * perpDist;
return (c0 + c1) * sourceLength;
}
private void UpdateMoveTowardsDestination(float speed)
{
if (!ReachedDestination)
{
var direction = CurrentWaypoint - Entity.Transform.WorldMatrix.TranslationVector;
// Get distance towards next point and normalize the direction at the same time
var length = direction.Length();
direction /= length;
bool advance = false;
// Check to see if an intermediate point was passed by projecting the position along the path
if (pathToDestination.Count > 0 && waypointIndex > 0 && waypointIndex != pathToDestination.Count - 1)
{
Vector3 pointNormal = CurrentWaypoint - pathToDestination[waypointIndex - 1];
pointNormal.Normalize();
float current = Vector3.Dot(Entity.Transform.WorldMatrix.TranslationVector, pointNormal);
float target = Vector3.Dot(CurrentWaypoint, pointNormal);
if (current > target)
{
advance = true;
}
}
else
{
if (length < DestinationThreshold)
{
advance = true;
}
}
if (advance)
{
waypointIndex++;
if (ReachedDestination)
{
HaltMovement();
return;
}
}
// Calculate speed based on distance from final destination
float moveSpeed = (moveDestination - Entity.Transform.WorldMatrix.TranslationVector).Length() * DestinationSlowdown;
if (moveSpeed > 1.0f)
{
moveSpeed = 1.0f;
}
// Slow down around corners
float cornerSpeedMultiply = Math.Max(0.0f, Vector3.Dot(direction, moveDirection)) * CornerSlowdown + (1.0f - CornerSlowdown);
// Allow a very simple inertia to the character to make animation transitions more fluid
moveDirection = moveDirection * 0.85f + direction * moveSpeed * cornerSpeedMultiply * 0.15f;
character.SetVelocity(moveDirection * speed);
// Broadcast speed as per cent of the max speed
RunSpeedEventKey.Broadcast(moveDirection.Length());
// Character orientation
if (moveDirection.Length() > 0.001)
{
lastYawOrientation = yawOrientation;
yawOrientation = MathUtil.RadiansToDegrees((float)Math.Atan2(-moveDirection.Z, moveDirection.X) + MathUtil.PiOverTwo);
}
modelChildEntity.Transform.Rotation = Quaternion.RotationYawPitchRoll(MathUtil.DegreesToRadians(yawOrientation), 0, 0);
}
else
{
HaltMovement();
}
}
public Vector3 calculateDragForce(polygon poly, Vector3 linearVelocity, Matrix3 oldRotation, Vector3 angularVelocity, Vector3 objectPosition)
{
Normal = Vector3.Cross(poly.vertices[0] - poly.vertices[1], poly.vertices[0] - poly.vertices[2]);
//if (Normal.Y > 0) Normal = -1.0f * Normal;
Velocity = new Vector3(0.0f, 0.0f, 0.0f);
if (usingLinearDrag)
{
Velocity -= linearVelocity;
}
if (usingAngularDrag)
{
Velocity -= Vector3.Cross(angularVelocity, oldRotation * poly.centerOfMass);
}
if (usingWind)
{
Velocity -= windSim.returnWind(oldRotation * poly.centerOfMass + objectPosition);
}
//Velocity = -linearVelocity - Vector3.Cross(angularVelocity, oldRotation * poly.centerOfMass) -windSim.returnWind(oldRotation * poly.centerOfMass + objectPosition);
//Console.WriteLine("Centroid: " + poly.centerOfMass.Length());
areaPercent = Math.Abs(Vector3.Dot(oldRotation * Normal, Velocity) / (Normal.Length() * Velocity.Length()));
//Console.WriteLine("poly Area: " + poly.area);
return (p * poly.area * areaPercent * 0.5f * Cd * Vector3.Multiply(Velocity, abs(Velocity)));
}
/// <summary>
/// 渲染控制点
/// </summary>
protected virtual void Render(DrawArgs drawArgs, GCP gcp, Vector3 projectedPoint)
{
//判断当前GCP是否被初始化了，比如，若重新定义了控制点的位置，则它的isInitialized就是false
if (!gcp.IsInitialized)
{
gcp.Initialize(drawArgs);
}
//判断当前控制点是否在视地范围内，若不在，则不进行绘制
if (!drawArgs.WorldCamera.ViewFrustum.ContainsPoint(gcp.Position))
{
return;
}
// 判断控制点是否在最大，最小可见的范围内，若不在，则不进行绘制
double distanceToGCP = Vector3.Length(gcp.Position - drawArgs.WorldCamera.Position);
if (distanceToGCP > gcp.MaximumDisplayDistance)
{
return;
}
if (distanceToGCP < gcp.MinimumDisplayDistance)
{
return;
}
//获得当前图层的纹理对象
GCPTexture gcpTexture = GetTexture(gcp);
//判断是否是MouseOver对象
bool isMouseOver = gcp == mouseOverGCP;
//若是MouseOver对象，则绘制Description里的内容
if (isMouseOver)
{
//若是MouseOver对象
isMouseOver = true;
//若当前图层可以操作，则设置当前的鼠标是Hand
if (gcp.IsSelectable)
{
DrawArgs.MouseCursor = DrawArgs.MouseCursor == CursorType.SizeAll ? CursorType.SizeAll : CursorType.Hand;
}
//显示文字描述信息,暂时不需要
string description = string.Format("纬度：{0:f6}°\n经度：{1:f6}°\n", gcp.Latitude, gcp.Longitude);
//绘制文字信息
if (description != null)
{
//设置文字的绘制区域
DrawTextFormat format = DrawTextFormat.NoClip | DrawTextFormat.WordBreak | DrawTextFormat.Bottom;
Rectangle rect = Rectangle.FromLTRB(DrawArgs.CurrentMousePosition.X,
DrawArgs.CurrentMousePosition.Y,
DrawArgs.CurrentMousePosition.X + 200, DrawArgs.CurrentMousePosition.Y + 60);
//绘制边框
drawArgs.DefaultDrawingFont.DrawText(
m_sprite, description,
rect,
format, 0xb0 << 24);
rect.Offset(2, 0);
drawArgs.DefaultDrawingFont.DrawText(
m_sprite, description,
rect,
format, 0xb0 << 24);
rect.Offset(0, 2);
drawArgs.DefaultDrawingFont.DrawText(
m_sprite, description,
rect,
format, 0xb0 << 24);
rect.Offset(-2, 0);
drawArgs.DefaultDrawingFont.DrawText(
m_sprite, description,
rect,
format, 0xb0 << 24);
// 绘制文字信息
rect.Offset(1, -1);
drawArgs.DefaultDrawingFont.DrawText(
m_sprite, description,
rect,
format, descriptionColor);
}
}
//获取颜色
int color = isMouseOver ? hotColor : normalColor;
if (gcpTexture == null || isMouseOver || gcp.NameAlwaysVisible)
{
// 绘制控制点的名称
if (gcp.Name != null)
{
// Render name field
const int labelWidth = 1000; // Dummy value needed for centering the text
if (gcpTexture == null)
{
// Center over target as we have no bitmap
Rectangle rect = new Rectangle(
(int)projectedPoint.X - (labelWidth >> 1),
(int)(projectedPoint.Y - (drawArgs.GCPNameFont.Description.Height >> 1)),
labelWidth,
drawArgs.ScreenHeight);
drawArgs.GCPNameFont.DrawText(m_sprite, gcp.Name, rect, DrawTextFormat.Center, color);
}
else
{
// Adjust text to make room for GCP
int spacing = (int)(gcp.Width * 0.3f);
if (spacing > 10)
{
spacing = 10;
}
int offsetForGCP = (gcp.Width >> 1) + spacing;
Rectangle rect = new Rectangle(
(int)projectedPoint.X + offsetForGCP,
(int)(projectedPoint.Y - (drawArgs.GCPNameFont.Description.Height >> 1)),
labelWidth,
drawArgs.ScreenHeight);
drawArgs.GCPNameFont.DrawText(m_sprite, gcp.Name, rect, DrawTextFormat.WordBreak, color);
}
}
}
//绘制控制点
if (gcpTexture != null)
{
// Render GCP
float xscale = (float)gcp.Width / gcpTexture.Width;
float yscale = (float)gcp.Height / gcpTexture.Height;
m_sprite.Transform = Matrix.Scaling(xscale, yscale, 0);
if (gcp.IsRotated)
{
m_sprite.Transform *= Matrix.RotationZ((float)gcp.Rotation.Radians - (float)drawArgs.WorldCamera.Heading.Radians);
}
m_sprite.Transform *= Matrix.Translation(projectedPoint.X, projectedPoint.Y, 0);
m_sprite.Draw(gcpTexture.Texture,
new Vector3(gcpTexture.Width >> 1, gcpTexture.Height >> 1, 0),
Vector3.Empty,
color);
// Reset transform to prepare for text rendering later
m_sprite.Transform = Matrix.Identity;
}
}
public override bool IsPointInside(Vector3 particlePosition, out Vector3 surfacePoint, out Vector3 surfaceNormal)
{
particlePosition -= fieldPosition;
inverseRotation.Rotate(ref particlePosition);
particlePosition /= fieldSize;
var maxDist = particlePosition.Length() / radius;
if (maxDist <= MathUtil.ZeroTolerance)
{
surfacePoint = fieldPosition;
surfaceNormal = new Vector3(0, 1, 0);
return true;
}
surfaceNormal = particlePosition / maxDist;
surfacePoint = surfaceNormal;
surfacePoint *= fieldSize;
fieldRotation.Rotate(ref surfacePoint);
surfacePoint += fieldPosition;
surfaceNormal /= fieldSize;
fieldRotation.Rotate(ref surfaceNormal);
surfaceNormal.Normalize();
return (maxDist <= 1);
}
///<summary>动画移动相机查看指定位置,按内部三维坐标, timerFactor: 速度系数, 为0则无动画直接刷新</summary>
public void aniLook(VECTOR3D vecLocation, double speedFactor = 1)
{
//计算当前与地面夹角
System.Windows.Media.Media3D.Vector3D v1, v2, v3, v4, v5;
Vector3 vec = new Vector3(vecLocation.x, vecLocation.y, vecLocation.z);
if (earth.earthManager.earthpara.SceneMode == ESceneMode.地球)
{
vec = vec / vec.Length() * Para.Radius; //换算到地表高度
}
//当前观察点
Vector3?crosspnt;
float? distance;
float height;
getCrossGround(out crosspnt, out distance);
if (crosspnt != null)
{
float dis = (float)distance;
Vector3 cp = (Vector3)crosspnt;
if (earth.earthManager.earthpara.SceneMode == ESceneMode.地球)
{
v1 = new System.Windows.Media.Media3D.Vector3D(cp.X, cp.Y, cp.Z);
v2 = new System.Windows.Media.Media3D.Vector3D(cameraUp.X, cameraUp.Y, cameraUp.Z);
v3 = System.Windows.Media.Media3D.Vector3D.CrossProduct(v1, v2);
v4 = System.Windows.Media.Media3D.Vector3D.CrossProduct(v3, v1);
v5 = new System.Windows.Media.Media3D.Vector3D(cameraDirection.X, cameraDirection.Y, cameraDirection.Z);
float angle = (float)System.Windows.Media.Media3D.Vector3D.AngleBetween(v5, v4);
height = (new Vector3(cameraPosition.X, cameraPosition.Y, cameraPosition.Z)).Length() - Para.Radius;
Vector3 dir = vec;
dir.Normalize();
Vector3 axis = Vector3.Cross(dir, cameraUp);
axis.Normalize();
Matrix matrix = Matrix.CreateFromAxisAngle(axis, -MathHelper.Pi * (90.0f - angle) / 180.0f);
dir = Vector3.Transform(dir, matrix);
dir.Normalize();
cameraLookat = vec;
cameraPosition = cameraLookat + dir * dis;
cameraDirection = cameraLookat - cameraPosition;
cameraDirection.Normalize();
}
else
{
Matrix matrix = Matrix.CreateTranslation(vecLocation.x - cp.X, vecLocation.y - cp.Y, 0);
cameraPosition = Vector3.Transform(cameraPosition, matrix);
height = cameraPosition.Z;
}
if (speedFactor == 0)
{
calCameraByDirection();
updateD3DCamera();
earth.global.isUpdate = true;
}
else
{
int time = (int)((cp - cameraLookat).Length() / height / speedFactor * 100); //时长，与高度反比，和速度系数反比
calCameraByDirection();
updateD3DCamera(true, time);
tmr.Start();
}
}
}
public void Vector3LengthTest1()
{
Vector3 target = new Vector3();
float expected = 0.0f;
float actual = target.Length();
Assert.IsTrue(MathHelper.Equal(expected, actual), "Vector3.Length did not return the expected value.");
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List <TgcBoundingBox> obstaculos, int recursionDepth)
{
//Limitar recursividad
if (recursionDepth > 5)
{
return;
}
//Ver si la distancia a recorrer es para tener en cuenta
float distanceToTravelSq = movementVector.LengthSq();
if (distanceToTravelSq < EPSILON)
{
return;
}
//Posicion deseada
Vector3 originalSphereCenter = characterSphere.Center;
Vector3 nextSphereCenter = originalSphereCenter + movementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
float minCollisionDistSq = float.MaxValue;
Vector3 realMovementVector = movementVector;
TgcBoundingBox.Face collisionFace = null;
TgcBoundingBox collisionObstacle = null;
Vector3 nearestPolygonIntersectionPoint = Vector3.Empty;
foreach (TgcBoundingBox obstaculoBB in obstaculos)
{
//Obtener los polígonos que conforman las 6 caras del BoundingBox
TgcBoundingBox.Face[] bbFaces = obstaculoBB.computeFaces();
foreach (TgcBoundingBox.Face bbFace in bbFaces)
{
Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane);
TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector);
float brutePlaneDist;
Vector3 brutePlaneIntersectionPoint;
if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint))
{
continue;
}
float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq();
if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq)
{
continue;
}
//Obtener punto de colisión en el plano, según la normal del plano
float pDist;
Vector3 planeIntersectionPoint;
Vector3 sphereIntersectionPoint;
TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal);
bool embebbed = false;
bool collisionFound = false;
if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//Ver si el plano está embebido en la esfera
if (pDist <= characterSphere.Radius)
{
embebbed = true;
//TODO: REVISAR ESTO, caso embebido a analizar con más detalle
sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius;
}
//Esta fuera de la esfera
else
{
//Obtener punto de colisión del contorno de la esfera según la normal del plano
sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius);
//Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento
TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector);
if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//no hay colisión
continue;
}
}
//Ver si planeIntersectionPoint pertenece al polígono
Vector3 newMovementVector;
float newMoveDistSq;
Vector3 polygonIntersectionPoint;
if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace))
{
if (embebbed)
{
//TODO: REVISAR ESTO, nunca debería pasar
//throw new Exception("El polígono está dentro de la esfera");
}
polygonIntersectionPoint = planeIntersectionPoint;
collisionFound = true;
}
else
{
//Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara
polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint,
bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]);
//Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega
Vector3 reversePointSeg = polygonIntersectionPoint - movementVector;
if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint))
{
collisionFound = true;
}
}
if (collisionFound)
{
//Nuevo vector de movimiento acotado
newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint;
newMoveDistSq = newMovementVector.LengthSq();
if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq)
{
minCollisionDistSq = newMoveDistSq;
realMovementVector = newMovementVector;
nearestPolygonIntersectionPoint = polygonIntersectionPoint;
collisionFace = bbFace;
collisionObstacle = obstaculoBB;
}
}
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (collisionFace == null)
{
//Avanzar hasta muy cerca
float movementLength = movementVector.Length();
movementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(movementVector);
return;
}
//Solo movernos si ya no estamos muy cerca
if (minCollisionDistSq >= EPSILON)
{
//Mover el BoundingSphere hasta casi la nueva posición real
float movementLength = realMovementVector.Length();
realMovementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(realMovementVector);
}
//Calcular plano de Sliding
Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint;
Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint;
slidePlaneNormal.Normalize();
Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Proyectamos el punto original de destino en el plano de sliding
TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal);
float slideT;
Vector3 slideDestinationPoint;
if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint))
{
//Nuevo vector de movimiento
Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint;
if (slideMovementVector.LengthSq() < EPSILON)
{
return;
}
//Recursividad para aplicar sliding
doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1);
}
}
public static int testDotProduct(Vector3 v0)
{
float f1 = (float)random.NextDouble();
float f2 = (float)random.NextDouble();
float f3 = (float)random.NextDouble();
Vector3 v1 = Vector3.Normalize(getTestValue(f1, f2, f3) - v0);
Vector3 v2 = new Vector3(f1, f2, f3) - v0;
v2 = v2 / v2.Length();
if (!Check(v1.X, v2.X) || !Check(v1.Y, v2.Y) || !Check(v1.Z, v2.Z))
{
Console.WriteLine("Vectors do not match " + v1 + v2);
return -1;
}
return 100;
}
public float GetDistance()
{
return(TriggerPosition.Length());
}
private VertexPositionNormalTexture[] CreatePatchVertices(Vector3[] patch, int tessellation, bool isMirrored)
{
var r = new List <VertexPositionNormalTexture>();
Debug.Assert(patch.Length == 16);
for (int i = 0; i <= tessellation; i++)
{
float ti = (float)i / tessellation;
for (int j = 0; j <= tessellation; j++)
{
float tj = (float)j / tessellation;
// Perform four horizontal bezier interpolations
// between the control points of this patch.
Vector3 p1 = Bezier(patch[0], patch[1], patch[2], patch[3], ti);
Vector3 p2 = Bezier(patch[4], patch[5], patch[6], patch[7], ti);
Vector3 p3 = Bezier(patch[8], patch[9], patch[10], patch[11], ti);
Vector3 p4 = Bezier(patch[12], patch[13], patch[14], patch[15], ti);
// Perform a vertical interpolation between the results of the
// previous horizontal interpolations, to compute the position.
Vector3 position = Bezier(p1, p2, p3, p4, tj);
// Perform another four bezier interpolations between the control
// points, but this time vertically rather than horizontally.
Vector3 q1 = Bezier(patch[0], patch[4], patch[8], patch[12], tj);
Vector3 q2 = Bezier(patch[1], patch[5], patch[9], patch[13], tj);
Vector3 q3 = Bezier(patch[2], patch[6], patch[10], patch[14], tj);
Vector3 q4 = Bezier(patch[3], patch[7], patch[11], patch[15], tj);
// Compute vertical and horizontal tangent vectors.
Vector3 tangentA = BezierTangent(p1, p2, p3, p4, tj);
Vector3 tangentB = BezierTangent(q1, q2, q3, q4, ti);
// Cross the two tangent vectors to compute the normal.
Vector3 normal = Vector3.Cross(tangentA, tangentB);
if (normal.Length() > 0.0001f)
{
normal.Normalize();
// If this patch is mirrored, we must invert the normal.
if (isMirrored)
{
normal = -normal;
}
}
else
{
// In a tidy and well constructed bezier patch, the preceding
// normal computation will always work. But the classic teapot
// model is not tidy or well constructed! At the top and bottom
// of the teapot, it contains degenerate geometry where a patch
// has several control points in the same place, which causes
// the tangent computation to fail and produce a zero normal.
// We 'fix' these cases by just hard-coding a normal that points
// either straight up or straight down, depending on whether we
// are on the top or bottom of the teapot. This is not a robust
// solution for all possible degenerate bezier patches, but hey,
// it's good enough to make the teapot work correctly!
//if (position.Y > 0)
normal = Vector3.Up;
//else
// normal = Vector3.Down;
}
// Create the vertex.
r.Add(new VertexPositionNormalTexture()
{
Position = position, Normal = normal
});
}
}
return(r.ToArray());
}
/// <summary>
/// Finds the distance between two points in 3D space.
/// </summary>
/// <param name="posFirst">First position</param>
/// <param name="posSecond">Second position</param>
/// <returns>Distance between the two positions.</returns>
public static float DistancePointToPoint(ref Vector3 posFirst, ref Vector3 posSecond)
{
Vector3 diffVect = posFirst - posSecond;
return(diffVect.Length());
}
