public TerrainFace(ShapeGenerator shapeGenerator, Mesh mesh, int resolution, Vector3 localUp)
{
this.shapeGenerator = shapeGenerator;
this.mesh = mesh;
this.resolution = resolution;
this.localUp = localUp;
axisA = new Vector3(localUp.y, localUp.z, localUp.x);
axisB = Vector3.Cross(localUp, axisA);
}
/// <summary>Calcule distance minimum entre les deux droites</summary>
public static float Dist(Line x, Line y)
{
Point n = Point.Cross(x.direction.normalized, y.direction.normalized);
if (n.magnitude == 0) // if parallel
{
return(Dist(x, y.origin));
}
return(Mathf.Abs(Point.Dot(n.normalized, (y.origin - x.origin))));
}
private void _getMarbleAxis(out Vector3 sideDir, out Vector3 motionDir, out Vector3 upDir)
{
var m = Quaternion.Euler(CameraY, 0, 0) * Quaternion.Euler(0, CameraX, 0);
upDir = -GravityDir;
motionDir = m * _forwards;
sideDir = Vector3.Cross(motionDir, upDir);
sideDir.Normalize();
motionDir = Vector3.Cross(upDir, sideDir);
}
//calculates a vector that is facing forward but propduces an angular velocity around the middle of the swarm
private Vector3 CalculateVortexingVelocity()
{
Vector3 _vecFromMiddle = transform.position - _vortexMiddle;
Vector3 output = Vector3.Cross(_vecFromMiddle.normalized, _rb.velocity.normalized) * _vortexLevel;
if (slythTraits.showVortexingVector)
{
Debug.DrawRay(transform.position, output, Color.magenta);
}
return(output);
}
/** True if the matrix will reverse orientations of faces.
*
* Scaling by a negative value along an odd number of axes will reverse
* the orientation of e.g faces on a mesh. This must be counter adjusted
* by for example the recast rasterization system to be able to handle
* meshes with negative scales properly.
*
* We can find out if they are flipped by finding out how the signed
* volume of a unit cube is transformed when applying the matrix
*
* If the (signed) volume turns out to be negative
* that also means that the orientation of it has been reversed.
*
* \see https://en.wikipedia.org/wiki/Normal_(geometry)
* \see https://en.wikipedia.org/wiki/Parallelepiped
*/
public static bool ReversesFaceOrientations(Matrix4x4 matrix)
{
var dX = matrix.MultiplyVector(new Vector3(1, 0, 0));
var dY = matrix.MultiplyVector(new Vector3(0, 1, 0));
var dZ = matrix.MultiplyVector(new Vector3(0, 0, 1));
// Calculate the signed volume of the parallelepiped
var volume = Vector3.Dot(Vector3.Cross(dX, dY), dZ);
return(volume < 0);
}
private bool CheckCollision(ParticleCollider particleCollider, Vector3 position, float radius, out Vector3 penetrationNormal, out Vector3 penetrationPosition, out float penetrationLength)
{
Vector3 colliderProjection = particleCollider.Position - position;
penetrationNormal = Vector3.Cross(particleCollider.Right, particleCollider.Up);
penetrationLength = Mathf.Abs(Vector3.Dot(colliderProjection, penetrationNormal)) - (radius / 2.0f);
penetrationPosition = particleCollider.Position - colliderProjection;
return(penetrationLength < 0.0f &&
Mathf.Abs(Vector3.Dot(colliderProjection, particleCollider.Right)) < particleCollider.Scale.x &&
Mathf.Abs(Vector3.Dot(colliderProjection, particleCollider.Up)) < particleCollider.Scale.y);
}
public void UpdateSliceNormal()
{
sliceNormal = Vector3.Cross(WorldNormal(), atom.transform.position - atom.molecularParent.transform.position);
if (sliceNormal.sqrMagnitude == 0)
{
sliceNormal = Vector3.Cross(WorldNormal(), Vector3.right);
}
if (sliceNormal.sqrMagnitude == 0)
{
sliceNormal = Vector3.Cross(WorldNormal(), Vector3.up);
}
}
private Vector3 EPA_2D(Prism prismA, Prism prismB)
{
var n = pointList.Count();
var minDist = float.MaxValue;
var prevDist = 0f;
Tuple <Vector3, Vector3> closestSide = null;
Vector3 support = Vector3.zero;
var normal = Vector3.zero;
List <Tuple <Vector3, Vector3> > simplex = new List <Tuple <Vector3, Vector3> >();
for (var i = 0; i < n; i++)
{
simplex.Add(new Tuple <Vector3, Vector3>(pointList[i], pointList[(i + 1) % n]));
}
while (Mathf.Abs(prevDist - minDist) > 0.01f)
{
// DrawSimplex(simplex);
prevDist = minDist;
foreach (var side in simplex)
{
float distanceFromOrigin = Vector3.Cross(side.Item2 - side.Item1, -side.Item1).magnitude;
if (distanceFromOrigin < minDist)
{
minDist = distanceFromOrigin;
closestSide = side;
}
}
// find normal to this vector in the outward direction (away from origin)
var direction = closestSide.Item2 - closestSide.Item1;
normal = new Vector3(-direction.z, 0f, direction.x);
if (Vector3.Dot(normal, -closestSide.Item1) > 0)
{
normal = -normal;
}
// find support point on minkowski in the direction of this normal vector
support = getSupport(prismA, prismB, normal);
// remove this side from from the list and add the two new sides in the list
simplex.Remove(closestSide);
simplex.Add(new Tuple <Vector3, Vector3>(closestSide.Item1, support - closestSide.Item1));
simplex.Add(new Tuple <Vector3, Vector3>(support, closestSide.Item2));
}
// Debug.DrawLine(Vector3.zero, support, Color.yellow,UPDATE_RATE);
return(support);
}
void AdjustVelocity()
{
float acceleration, speed;
Vector3 xAxis, zAxis;
if (Climbing)
{
acceleration = maxClimbAcceleration;
speed = maxClimbSpeed;
xAxis = Vector3.Cross(contactNormal, upAxis);
zAxis = upAxis;
}
else if (InWater)
{
float swimFactor = Mathf.Min(1f, submergence / swimThreshold);
acceleration = Mathf.LerpUnclamped(OnGround ? maxAcceleration : maxAirAcceleration,
maxSwimAcceleration, swimFactor);
speed = Mathf.LerpUnclamped(maxSpeed, maxSwimSpeed, swimFactor);
xAxis = rightAxis;
zAxis = forwardAxis;
}
else
{
acceleration = OnGround ? maxAcceleration : maxAirAcceleration;
speed = OnGround && desiresClimbing ? maxClimbSpeed : maxSpeed;
xAxis = rightAxis;
zAxis = forwardAxis;
}
float maxSpeedChange = acceleration * Time.deltaTime;
xAxis = ProjectOnContactPlane(xAxis, contactNormal);
zAxis = ProjectOnContactPlane(zAxis, contactNormal);
Vector3 relativeVelocity = velocity - connectionVelocity;
float currentX = Vector3.Dot(relativeVelocity, xAxis);
float currentZ = Vector3.Dot(relativeVelocity, zAxis);
float newX = Mathf.MoveTowards(currentX, playerInput.x * speed, maxSpeedChange);
float newZ = Mathf.MoveTowards(currentZ, playerInput.y * speed, maxSpeedChange);
velocity += xAxis * (newX - currentX) + zAxis * (newZ - currentZ);
if (Swimming)
{
float currentY = Vector3.Dot(relativeVelocity, upAxis);
float newY = Mathf.MoveTowards(currentY, playerInput.z * speed, maxSpeedChange);
velocity += upAxis * (newY - currentY);
}
}
private Vector3 VertexOfCircleEvent(CircleEvent circle, float angle)
{
var a = circle.LeftArc.Site.Position.ToUnityVector3();
var b = circle.MiddleArc.Site.Position.ToUnityVector3();
var c = circle.RightArc.Site.Position.ToUnityVector3();
var n = Vector3.Cross(a - b, c - b).normalized;
var s = a - Vector3.Dot(a, n) * n;
var t = Vector3.Cross(s, n);
var vertex = Vector3.Dot(a, n) * n + Mathf.Cos(angle) * s + Mathf.Sin(angle) * t;
return(vertex);
}
static private float Angle(Vector3 pos1, Vector3 pos2)
{
Vector3 from = pos2 - pos1;
Vector3 to = new Vector3(1, 0);
float result = Vector3.Angle(from, to);
Vector3 cross = Vector3.Cross(from, to);
if (cross.z > 0)
{
result = 360f - result;
}
return(result);
}
private List <Vector3> calculateSurfaceNormals()
{
List <Vector3> surfaceNormals = new List <Vector3>();
for (int i = 0; i < triangles.Count; i += 3)
{
Vector3 v0 = vertices[triangles[i]] - vertices[triangles[i + 2]];
Vector3 v1 = vertices[triangles[i + 1]] - vertices[triangles[i + 2]];
Vector3 surfaceNormal = Vector3.Cross(v0, v1);
surfaceNormals.Add(surfaceNormal.normalized);
}
return(surfaceNormals);
}
private static IEnumerable <Vector3> VerticesInEdge(Vertex origin, Vertex destination)
{
var originVector = origin.Position.ToUnityVector3();
var destinationVector = destination.Position.ToUnityVector3();
if (originVector == destinationVector || Vector3.Cross(originVector, destinationVector) != new Vector3(0, 0, 0))
{
return(DrawingUtilities.VerticesOnGeodesic(originVector, destinationVector, NumberOfVerticesPerEdge));
}
else
{
var commonSites = origin.Sites.Intersect(destination.Sites).ToList();
var normal = -(commonSites.First().Position.ToUnityVector3() - commonSites.Last().Position.ToUnityVector3()).normalized;
return(DrawingUtilities.VerticesOnHalfOfGreatCircle(originVector, normal, NumberOfVerticesPerEdge));
}
}
public override void GetPath(Turn turn, List <Vector3> output)
{
AddCircleSegment(gamma1, gamma2, clockwise, circleCenter, output, (circleCenter - current).magnitude);
normal = (current - circleCenter).normalized;
t2 = Vector3.Cross(normal, Vector3.up).normalized;
normal = -normal;
if (!clockwise)
{
t2 = -t2;
normal = -normal;
}
changedPreviousTangent = true;
}
public static bool PointInTriangle(Vector3 pnt, Vector3 a, Vector3 b, Vector3 c)
{
a -= pnt;
b -= pnt;
c -= pnt;
Vector3 bc = Vector3.Cross(b, c);
Vector3 ca = Vector3.Cross(c, a);
if (Vector3.Dot(bc, ca) < 0.0)
{
return(false);
}
Vector3 ab = Vector3.Cross(a, b);
return(Vector3.Dot(bc, ab) >= 0.0);
}
private bool _computeMoveForces(Vector3 angVelocity, out Vector3 torque, out Vector3 targetAngVel)
{
torque = Vector3.zero;
targetAngVel = Vector3.zero;
Vector3 relGravity = -GravityDir * Radius;
Vector3 topVelocity = Vector3.Cross(angVelocity, relGravity);
_getMarbleAxis(out var sideDir, out var motionDir, out Vector3 _);
float topY = Vector3.Dot(topVelocity, motionDir);
float topX = Vector3.Dot(topVelocity, sideDir);
Vector2 move = InputMovement;
// move.Normalize();
float moveY = MaxRollVelocity * move.y;
float moveX = MaxRollVelocity * move.x;
if (Math.Abs(moveY) < 0.001f && Math.Abs(moveX) < 0.001f)
{
return(false);
}
if (topY > moveY && moveY > 0.0)
{
moveY = topY;
}
else if (topY < moveY && moveY < 0.0)
{
moveY = topY;
}
if (topX > moveX && moveX > 0.0)
{
moveX = topX;
}
else if (topX < moveX && moveX < 0.0)
{
moveX = topX;
}
targetAngVel = Vector3.Cross(relGravity, moveY * motionDir + moveX * sideDir) / relGravity.sqrMagnitude;
torque = targetAngVel - angVelocity;
float targetAngAccel = torque.magnitude;
if (targetAngAccel > AngularAcceleration)
{
torque *= AngularAcceleration / targetAngAccel;
}
return(true);
}
public void Cylinder(Vector3 position, Vector3 up, float height, float radius, Color color)
{
var tangent = Vector3.Cross(up, Vector3.one).normalized;
matrix = Matrix4x4.TRS(position, Quaternion.LookRotation(tangent, up), new Vector3(radius, height, radius));
CircleXZ(Vector3.zero, 1, color);
if (height > 0)
{
CircleXZ(Vector3.up, 1, color);
Line(new Vector3(1, 0, 0), new Vector3(1, 1, 0), color);
Line(new Vector3(-1, 0, 0), new Vector3(-1, 1, 0), color);
Line(new Vector3(0, 0, 1), new Vector3(0, 1, 1), color);
Line(new Vector3(0, 0, -1), new Vector3(0, 1, -1), color);
}
matrix = Matrix4x4.identity;
}
//abstract void Evaluate (Turn turn);
public static void Setup(int i, Vector3[] vectorPath)
{
current = vectorPath[i];
prev = vectorPath[i - 1];
next = vectorPath[i + 1];
prev.y = current.y;
next.y = current.y;
t1 = t2;
t2 = (next - current).normalized - (prev - current).normalized;
t2 = t2.normalized;
prevNormal = normal;
normal = Vector3.Cross(t2, Vector3.up);
normal = normal.normalized;
}
void dodgePit()
{
Vector3 myPos = this.transform.position;
Vector3 perpendicular = Vector3.Cross(pitDir, Vector3.forward);
Vector3 velocityToAdd = perpendicular.normalized * speed * Time.deltaTime;
if (dodgingLeft)
{
velocityToAdd = -velocityToAdd;
}
this.transform.position = new Vector3(myPos.x + velocityToAdd.x, myPos.y + velocityToAdd.y, 0);
dodgeCounter--;
if (dodgeCounter <= 0)
{
dodgingPit = false;
}
}
//получение возможных разделяющих осей
private static List <Vector3> GetAxis(Vector3[] a, Vector3[] b)
{
//ребра
Vector3 A;
Vector3 B;
//потенциальные разделяющие оси
List <Vector3> Axis = new List <Vector3>();
//нормали плоскостей первого куба
for (int i = 1; i < 4; i++)
{
A = a[i] - a[0];
B = a[(i + 1) % 3 + 1] - a[0];
Axis.Add(Vector3.Cross(A, B).normalized);
}
//нормали второго куба
for (int i = 1; i < 4; i++)
{
A = b[i] - b[0];
B = b[(i + 1) % 3 + 1] - b[0];
Axis.Add(Vector3.Cross(A, B).normalized);
}
//Теперь добавляем все векторные произведения
for (int i = 1; i < 4; i++)
{
A = a[i] - a[0];
for (int j = 1; j < 4; j++)
{
B = b[j] - b[0];
if (Vector3.Cross(A, B).magnitude != 0)
{
Axis.Add(Vector3.Cross(A, B).normalized);
}
}
}
return(Axis);
}
public override void Draw()
{
base.Draw();
var delta = End - Position;
float len = delta.magnitude;
var dir = delta.normalized;
float angSin = Mathf.Sin(Mathf.Deg2Rad * Angle);
float radius = angSin * len;
Handles.color = Color;
var perp2d = Vector2.Perpendicular(new Vector2(dir.x, dir.z));
var perpendicular = Vector3.Cross(dir, new Vector3(perp2d.x, 0, perp2d.y)).normalized;
var start = Quaternion.AngleAxis(-Angle * 0.5f, perpendicular) * dir;
var end = Quaternion.AngleAxis(Angle * 0.5f, perpendicular) * dir;
Handles.DrawWireArc(Position, perpendicular, start, Angle, len);
Handles.DrawLine(Position, Position + start * len);
Handles.DrawLine(Position, Position + end * len);
}
private void CalculateFriction()
{
var isGrounded = _wheelCollisionDetector.TryGetCollision(out var point, out var normal, out var collider, out var rb, out var t);
if (!isGrounded)
{
return;
}
var forceOffset = transform.up * _applyForcesOffset;
var right = transform.right;
var forward = transform.forward;
var direction = Vector3.Cross(normal, right);
if (motorTorque < 0)
{
direction *= -1;
}
var lateralVelocity = Vector3.Project(velocity, right);
var forwardVelocity = Vector3.Project(velocity, forward);
var slip = (forwardVelocity + lateralVelocity) / 2;
var lateralFriction = Vector3.Project(right, slip).magnitude *lateralVelocity.magnitude *lateralFrictionMultiplier / Time.fixedDeltaTime;
_rb.AddForceAtPosition(-Vector3.Project(slip, lateralVelocity).normalized *lateralFriction, point + forceOffset);
var motorForce = Mathf.Abs(motorTorque / _wheel.GetRadius());
var maxForwardFriction = motorForce * forwardFrictionCoeff;
var appliedForwardFriction = Mathf.Clamp(motorForce, 0, maxForwardFriction);
var forwardForce = direction.normalized * appliedForwardFriction * engineShaftToWheelRatio;
_rb.AddForceAtPosition(forwardForce, point + forceOffset);
}
/*
* Method for Handling Zoom and Rotate
*/
private void ZoomRotate()
{
var touchZero = Input.touches[0];
var touchOne = Input.touches[1];
if (touchZero.phase == TouchPhase.Began || touchOne.phase == TouchPhase.Began)
{
_initPinchDist = Vector3.Distance(touchZero.position, touchOne.position);
_initRotation = touchZero.position - touchOne.position;
}
if (touchZero.phase == TouchPhase.Moved || touchOne.phase == TouchPhase.Moved)
{
var newPinchDist = Vector3.Distance(touchZero.position, touchOne.position);
_pinchDistDelta = newPinchDist - _initPinchDist; // save pinch difference
var rotationVector = touchZero.position - touchOne.position;
var rotationAngle = Vector3.Angle(rotationVector, _initRotation);
var cross = Vector3.Cross(_initRotation, rotationVector);
if (rotationAngle > RotationThreshold)
{
if (cross.z > 0)
{
RotateCamera(rotationAngle);
}
else if (cross.z < 0)
{
RotateCamera(-rotationAngle);
}
}
else if (Math.Abs(_pinchDistDelta) >= PinchThreshold)
{
ZoomCamera(touchZero, touchOne);
}
}
}
private void _applyContactForces(
float dt,
List <CollisionInfo> contacts,
bool isCentered,
Vector3 aControl,
Vector3 desiredOmega,
ref Vector3 velocity,
ref Vector3 omega,
ref Vector3 linAccel,
out Vector3 angAccel)
{
angAccel = Vector3.zero;
_slipAmount = 0.0f;
Vector3 vector31 = GravityDir;
int index1 = -1;
float num1 = 0.0f;
for (int index2 = 0; index2 < contacts.Count; ++index2)
{
// if (contacts[index2].collider == null)
// {
float num2 = -Vector3.Dot(contacts[index2].Normal, linAccel);
if (num2 > num1)
{
num1 = num2;
index1 = index2;
}
// }
}
CollisionInfo collisionInfo = index1 != -1 ? contacts[index1] : new CollisionInfo();
if (index1 != -1 && Jump)
{
Vector3 vector32 = velocity - collisionInfo.Velocity;
float num2 = Vector3.Dot(collisionInfo.Normal, vector32);
if (num2 < 0.0)
{
num2 = 0.0f;
}
if (num2 < JumpImpulse)
{
velocity += collisionInfo.Normal * (JumpImpulse - num2);
// MarbleControlComponent._soundBank.PlayCue(MarbleControlComponent._sounds[12]);
}
}
for (int index2 = 0; index2 < contacts.Count; ++index2)
{
float num2 = -Vector3.Dot(contacts[index2].Normal, linAccel);
if (num2 > 0.0 && Vector3.Dot(contacts[index2].Normal, velocity - contacts[index2].Velocity) <=
0.00001)
{
linAccel += contacts[index2].Normal * num2;
}
}
if (index1 != -1)
{
// (collisionInfo.velocity - (collisionInfo.normal * Vector3.Dot(collisionInfo.normal, collisionInfo.velocity)));
Vector3 vector32 = velocity + Vector3.Cross(omega, -collisionInfo.Normal * Radius) - collisionInfo.Velocity;
float num2 = vector32.magnitude;
bool flag = false;
Vector3 vector33 = new Vector3(0.0f, 0.0f, 0.0f);
Vector3 vector34 = new Vector3(0.0f, 0.0f, 0.0f);
if (num2 != 0.0)
{
flag = true;
float num3 = KineticFriction * collisionInfo.Friction;
float num4 = (float)(5.0 * num3 * num1 / (2.0 * Radius));
float num5 = num1 * num3;
float num6 = (num4 * Radius + num5) * dt;
if (num6 > num2)
{
float num7 = num2 / num6;
num4 *= num7;
num5 *= num7;
flag = false;
}
Vector3 vector35 = vector32 / num2;
vector33 = num4 * Vector3.Cross(-collisionInfo.Normal, -vector35);
vector34 = -num5 * vector35;
_slipAmount = num2 - num6;
}
if (!flag)
{
Vector3 vector35 = -vector31 * Radius;
Vector3 vector36 = Vector3.Cross(vector35, linAccel) / vector35.sqrMagnitude;
if (isCentered)
{
Vector3 vector37 = omega + angAccel * dt;
aControl = desiredOmega - vector37;
float num3 = aControl.magnitude;
if (num3 > BrakingAcceleration)
{
aControl = aControl * BrakingAcceleration / num3;
}
}
Vector3 vector38 = -Vector3.Cross(aControl, -collisionInfo.Normal * Radius);
Vector3 vector39 = Vector3.Cross(vector36, -collisionInfo.Normal * Radius) + vector38;
float num4 = vector39.magnitude;
float num5 = StaticFriction * collisionInfo.Friction;
if (num4 > num5 * num1)
{
float num3 = KineticFriction * collisionInfo.Friction;
vector38 *= num3 * num1 / num4;
}
linAccel += vector38;
angAccel += vector36;
}
linAccel += vector34;
angAccel += vector33;
}
angAccel += aControl;
}
private void _velocityCancel(List <CollisionInfo> contacts, ref Vector3 velocity, ref Vector3 omega, bool surfaceSlide, bool noBounce)
{
bool flag1 = false;
int iterations = 0;
bool done = false;
while (!done)
{
done = true;
++iterations;
foreach (var coll in contacts)
{
Vector3 relativeVelocity = velocity - coll.Velocity;
float bounceSpeed = Vector3.Dot(coll.Normal, relativeVelocity);
if (!flag1 && bounceSpeed < 0.0 || bounceSpeed < -0.001f)
{
Vector3 invBounce = bounceSpeed * coll.Normal;
// _reportBounce(contacts[index].point, contacts[index].normal, -num3);
if (noBounce)
{
velocity -= invBounce;
}
else if (coll.Collider != null)
{
CollisionInfo contact = coll;
if (false && contact.Collider.GetComponent <Movement>() is Movement owner)
{
float num5 = 1f;
float num6 = 1f;
float num7 = 0.5f;
var vector3_4 =
(
(
Vector3.Dot(
(
(velocity * num5) -
(owner.Velocity * num6)
),
contact.Normal
) *
contact.Normal
) *
(1f + num7)
);
/*Vector3.op_Multiply(
* Vector3.op_Multiply(
* Vector3.Dot(
* Vector3.op_Subtraction(
* Vector3.op_Multiply(velocity, num5),
* Vector3.op_Multiply(owner.Velocity, num6)
* ),
* contact.normal
* ),
* contact.normal
* ),
* 1f + num7
* );*/
velocity = velocity - (vector3_4 / num5);
//velocity = Vector3.op_Subtraction(velocity, Vector3.op_Division(vector3_4, num5));
var moveComponent = owner;
moveComponent.Velocity = moveComponent.Velocity + (vector3_4 / num6);
//moveComponent.Velocity = Vector3.op_Addition(moveComponent.Velocity, Vector3.op_Division(vector3_4, num6));
contact.Velocity = owner.Velocity;
//contacts[index] = contact; // ?
}
else
{
float num5 = 0.5f;
Vector3 vector34 = contact.Normal * (Vector3.Dot(velocity, contact.Normal) * (1f + num5));
velocity -= vector34;
}
}
else
{
if (coll.Velocity.magnitude > 0.00001 && !surfaceSlide &&
bounceSpeed > -MaxDotSlide * velocity.magnitude)
{
velocity -= invBounce;
velocity.Normalize();
velocity *= velocity.magnitude;
surfaceSlide = true;
}
else if (bounceSpeed > -MinBounceVel)
{
velocity -= invBounce;
}
else
{
Vector3 velocityAdd = (float)-(1.0 + BounceRestitution * coll.Restitution) * invBounce;
Vector3 velocityAtContact = relativeVelocity + Vector3.Cross(omega, -coll.Normal * Radius);
float num5 = -Vector3.Dot(coll.Normal, relativeVelocity);
Vector3 vector36 = velocityAtContact - coll.Normal * Vector3.Dot(coll.Normal, relativeVelocity);
float num6 = vector36.magnitude;
if (Math.Abs(num6) > 0.001f)
{
float inertia = (float)(5.0 * (BounceKineticFriction * coll.Friction) * num5 /
(2.0 * Radius));
if (inertia > num6 / Radius)
{
inertia = num6 / Radius;
}
Vector3 vector37 = vector36 / num6;
Vector3 vAtC = Vector3.Cross(-coll.Normal, -vector37);
Vector3 vector38 = inertia * vAtC;
omega += vector38;
velocity -= Vector3.Cross(-vector38, -coll.Normal * Radius);
}
velocity += velocityAdd;
}
}
done = false;
}
}
flag1 = true;
if (iterations > 6 && noBounce)
{
done = true;
}
if (iterations > 1e7)
{
Debug.Log("Collision lock");
break;
}
}
if (velocity.sqrMagnitude >= 625.0)
{
return;
}
bool flag3 = false;
Vector3 vector39 = new Vector3(0.0f, 0.0f, 0.0f);
for (int index = 0; index < contacts.Count; ++index)
{
Vector3 vector32 = vector39 + contacts[index].Normal;
if (vector32.sqrMagnitude < 0.01)
{
vector32 += contacts[index].Normal;
}
vector39 = vector32;
flag3 = true;
}
if (!flag3)
{
return;
}
vector39.Normalize();
float num8 = 0.0f;
for (int index = 0; index < contacts.Count; ++index)
{
if (contacts[index].Penetration < Radius)
{
float num3 = 0.1f;
float penetration = contacts[index].Penetration;
float num4 = Vector3.Dot(velocity + num8 * vector39, contacts[index].Normal);
if (num3 * num4 < penetration)
{
num8 += (penetration - num4 * num3) / num3 / Vector3.Dot(contacts[index].Normal, vector39);
}
}
}
float num9 = Mathf.Clamp(num8, -25f, 25f);
velocity += num9 * vector39;
}
void FixedUpdate()
{
ForceHeightLevel();
//Must be the local player, or they cannot move
if (!isLocalPlayer)
{
return;
}
//Ignore movement controls when typing in chat
if (chatRegister.ChatWindow != null && chatRegister.ChatWindow.PlayerIsTyping())
{
currentMovement.Set(0, 0);
return;
}
if (Input.GetButtonDown("Toggle Run"))
{
isWalking = !isWalking;
}
// TODO: Implement gravity and grabbing
// Calculate next movement
// The vector is not normalized to allow for the input having potential rise and fall times
float x = Input.GetAxisRaw("Horizontal");
float y = Input.GetAxisRaw("Vertical");
// Smoothly transition to next intended movement
intendedMovement = new Vector2(x, y).normalized *(isWalking ? walkSpeed : runSpeed);
currentMovement = Vector2.Lerp(currentMovement, intendedMovement, Time.deltaTime * (Mathf.Pow(ACCELERATION / 9.5f, 3) / 6));
// Move (without gravity). Whenever we move we also readjust the player's direction to the direction they are running in.
characterController.Move(absoluteMovement * Time.deltaTime);
// Move the player
if (currentMovement != Vector2.zero)
{
// Determine the absolute movement by aligning input to the camera's looking direction
Vector3 absoluteMovement =
currentMovement.y * Vector3.Cross(mainCamera.transform.right, Vector3.up).normalized +
currentMovement.x * Vector3.Cross(Vector3.up, mainCamera.transform.forward).normalized;
if (intendedMovement != Vector2.zero)
{
//absoluteMovement = Vector3.Lerp(absoluteMovement, newAbsoluteMovement, Time.deltaTime * 2);
// Move (without gravity). Whenever we move we also readjust the player's direction to the direction they are running in.
characterController.Move(absoluteMovement * Time.deltaTime);
// avoid unwanted rotation when you rotate the camera but isn't doing movement input, comment the "if" to see it
// Rotate the chest and head IKs objects
Quaternion newChestIKRotation = Quaternion.Lerp(chestIK.rotation, Quaternion.LookRotation(absoluteMovement), Time.deltaTime * (isWalking ? 3 : 10));
Quaternion newHeadIKRotation = Quaternion.Lerp(headIK.rotation, Quaternion.LookRotation(absoluteMovement), Time.deltaTime * (isWalking ? 15 : 5));
//float rotationDiference = Quaternion.
transform.rotation = Quaternion.Slerp(transform.rotation, Quaternion.LookRotation(absoluteMovement), Time.deltaTime * (isWalking ? 5 : 7));
//Mathf.Pow(intendedMovement.magnitude, 2)
chestIK.rotation = newChestIKRotation;
headIK.rotation = newHeadIKRotation;
}
if (intendedMovement == Vector2.zero)
{
Quaternion newChestIKRotation = Quaternion.Lerp(chestIK.rotation, transform.rotation, Time.deltaTime * 3);
Quaternion newHeadIKRotation = Quaternion.Lerp(headIK.rotation, transform.rotation, Time.deltaTime * 8);
absoluteMovement = Vector3.Lerp(absoluteMovement, Vector3.zero, Time.deltaTime * 5);
characterController.Move(absoluteMovement * Time.deltaTime);
chestIK.rotation = newChestIKRotation;
headIK.rotation = newHeadIKRotation;
}
}
}
void _advancePhysics(ref float dt)
{
List <CollisionInfo> contacts = new List <CollisionInfo>();
Vector3 pos = transform.position;
Quaternion rot = transform.rotation;
Vector3 velocity = Velocity;
Vector3 omega = AngularVelocity;
if (useUnityContacts)
{
foreach (var collision in _unityCollisions)
{
foreach (var contact in collision.contacts)
{
var penetration = -contact.separation;
var col = new CollisionInfo {
Penetration = penetration,
Restitution = 1,
Friction = 1,
Normal = contact.normal,
Point = contact.point
};
if (contact.otherCollider.attachedRigidbody != null)
{
col.Collider = contact.otherCollider;
col.Velocity = contact.otherCollider.attachedRigidbody.GetPointVelocity(contact.point);
}
contacts.Add(col);
Debug.DrawRay(contact.point, contact.normal, Color.blue, 5);
}
}
}
else
{
var radius = Radius + 0.0001f;
for (var index = 0; index < _colTests.Count; index++)
{
var meshCollider = _colTests[index];
var localPos = meshCollider.transform.InverseTransformPoint(pos);
var length = _meshes[index].Triangles.Length;
for (int i = 0; i < length; i += 3)
{
var triangle0 = _meshes[index].Triangles[i];
var triangle1 = _meshes[index].Triangles[i + 1];
var triangle2 = _meshes[index].Triangles[i + 2];
var p0 = _meshes[index].Vertices[triangle0];
var p1 = _meshes[index].Vertices[triangle1];
var p2 = _meshes[index].Vertices[triangle2];
var normal = Vector3.Cross(p2 - p0, p2 - p1).normalized;
if (CollisionHelpers.ClosestPtPointTriangle(localPos, radius, p0, p1, p2, normal, out var closest) == false)
{
continue;
}
Vector3 vector32 = localPos - closest;
if (vector32.sqrMagnitude > radius * radius)
{
continue;
}
Vector3 vector33 = localPos - closest;
if (Vector3.Dot(localPos - closest, normal) < 0.0)
{
continue;
}
vector33.Normalize();
CollisionInfo collisionInfo = new CollisionInfo()
{
Normal = meshCollider.transform.TransformDirection(vector33),
Point = meshCollider.transform.TransformPoint(closest)
};
Debug.DrawRay(collisionInfo.Point, collisionInfo.Normal, Color.red);
collisionInfo.Penetration = radius - Vector3.Dot(localPos - closest, collisionInfo.Normal);
collisionInfo.Restitution = 1f;
collisionInfo.Friction = 1f;
if (_colTests[index].attachedRigidbody != null)
{
collisionInfo.Velocity = _colTests[index].attachedRigidbody.GetPointVelocity(collisionInfo.Point);
}
contacts.Add(collisionInfo);
}
}
}
_updateMove(ref dt, ref velocity, ref omega, contacts);
// velocity += _gravityDir * _gravity * dt;
_updateIntegration(dt, ref pos, ref rot, velocity, omega);
if (useUnityContacts)
{
_rigidBody.MovePosition(pos);
_rigidBody.MoveRotation(rot);
}
else
{
transform.position = pos;
transform.rotation = rot;
}
Velocity = velocity;
AngularVelocity = omega;
}
private void OrbitParameters()
{
if (!moving)
{
return;
}
Vector3 r = orbitingBody.transform.position - transform.position;
radius = Vector3.Distance(orbitingBody.transform.position, transform.position);
float mass1 = rb.mass;
float mass2 = orbitingBody.GetComponent <Rigidbody>().mass;
float micro = g * mass2;
keneticEnergy = mass1 * velocity.magnitude * velocity.magnitude / 2;
potenetialEnergy = g * mass2 * mass1 / r.magnitude;
Energy = mass1 * velocity.magnitude * velocity.magnitude / 2 - g * mass2 * mass1 / r.magnitude;
float l = Vector3.Cross(velocity, r).magnitude;
init_energy = Energy;
float a = -g * mass2 * mass1 / (2 * Energy);
float T = (float)Math.Sqrt(4 * Math.PI * Math.PI * a * a * a / (g * mass2));
float e = (float)Math.Sqrt(1 + 2 * Energy * l * l / (mass1 * mass1 * mass1 * micro * micro));
ecentricity = e;
float per = (l * l / (mass1 * micro)) * (1 / (1 + e * (float)Math.Cos(0)));
apoapsis = (l * l / (mass1 * micro)) * (1 / (1 + e * (float)Math.Cos(Math.PI)));
periapsis = per;
//Does work if sat mass is not 1
orbital_period = T;
semiMajor = (apoapsis + periapsis) / 2;
semiMinor = Mathf.Sqrt(semiMajor * semiMajor * (1 - e * e));
float radi = radius;
theta = Mathf.Acos(l * l / (radi * mass1 * mass1 * micro * e) - 1 / e);
beta = Mathf.Acos(Vector3.Dot(new Vector3(1, 0, 0), r) / r.magnitude);
//theta = Mathf.Acos(l * l / (apoapsis * mass1 * mass1 * micro * e) - 1 / e);
print(Mathf.Rad2Deg * theta);
print(theta);
Vector3 p2 = new Vector3(radius * Mathf.Cos(theta), 0, radius * Mathf.Sin(Mathf.PI + theta));
//true if ccw (negative ccw)
renderer.ellipse.orbitSide = -1f;
print(Vector3.Dot(Vector3.Cross(r, velocity), new Vector3(0, 1, 0)));
float orbitSide = 1;
float clockWise = 1;
ccw = Vector3.Dot(Vector3.Cross(r, velocity), new Vector3(0, 1, 0)) > 0;
if (ccw)
{
clockWise *= -1;
//renderer.ellipse.orbitSide = 1f;
//p2 = new Vector3(radius * Mathf.Cos(theta), 0, radius * Mathf.Sin(theta));
//renderer.ellipse.orbitSide *= -1f;
//true if descending
if (Vector3.Dot(r, velocity) > 0)
{
orbitSide = -1;
}
}
else
{
orbitSide = -1;
if (Vector3.Dot(r, velocity) > 0)
{
orbitSide = 1;
}
}
//theta *= clockWise;
theta *= orbitSide;
beta *= -Mathf.Sign(r.z);
//renderer.ellipse.offsetAngle = -beta + theta - Mathf.PI;
renderer.ellipse.offsetAngle = -beta + Mathf.PI - theta;
//theta = Mathf.Acos((2 * radius * radius - Vector3.Distance(transform.position, p2)*Vector3.Distance(transform.position, p2)) /
// (2 * radius * radius));
//theta = Mathf.Acos((2 * radius * radius - Vector3.Distance(transform.position, p2)*Vector3.Distance(transform.position, p2)) / (2 * radius * radius));
print(Mathf.Rad2Deg * theta);
print(theta);
//print(Mathf.Rad2Deg* theta);
renderer.ellipse.offsetPoint = orbitingBody.transform.position;
renderer.ellipse.xAxis = semiMajor;
renderer.ellipse.yAxis = semiMinor;
renderer.ellipse.offset.x = orbitingBody.transform.position.x - semiMajor + periapsis;
renderer.ellipse.offset.y = orbitingBody.transform.position.z;
renderer.CalculateEllipse();
theta = Mathf.Rad2Deg * theta;
beta = Mathf.Rad2Deg * beta;
//What position in the orbit am I in? How do I return a future x,y cord?
}
/** Draws Gizmos */
public void OnDrawGizmos(bool selected)
{
gizmoDrawing = true;
Gizmos.color = new Color(0.615f, 1, 0.06f, selected ? 1.0f : 0.7f);
var movementPlane = RVOSimulator.active != null ? RVOSimulator.active.movementPlane : MovementPlane.XZ;
var up = movementPlane == MovementPlane.XZ ? Vector3.up : -Vector3.forward;
if (gizmoVerts == null || AreGizmosDirty() || _obstacleMode != obstacleMode)
{
_obstacleMode = obstacleMode;
if (gizmoVerts == null)
{
gizmoVerts = new List <Vector3[]>();
}
else
{
gizmoVerts.Clear();
}
CreateObstacles();
}
Matrix4x4 m = GetMatrix();
for (int i = 0; i < gizmoVerts.Count; i++)
{
Vector3[] verts = gizmoVerts[i];
for (int j = 0, q = verts.Length - 1; j < verts.Length; q = j++)
{
Gizmos.DrawLine(m.MultiplyPoint3x4(verts[j]), m.MultiplyPoint3x4(verts[q]));
}
if (selected)
{
for (int j = 0, q = verts.Length - 1; j < verts.Length; q = j++)
{
Vector3 a = m.MultiplyPoint3x4(verts[q]);
Vector3 b = m.MultiplyPoint3x4(verts[j]);
if (movementPlane != MovementPlane.XY)
{
Gizmos.DrawLine(a + up * Height, b + up * Height);
Gizmos.DrawLine(a, a + up * Height);
}
Vector3 avg = (a + b) * 0.5f;
Vector3 tang = (b - a).normalized;
if (tang == Vector3.zero)
{
continue;
}
Vector3 normal = Vector3.Cross(up, tang);
Gizmos.DrawLine(avg, avg + normal);
Gizmos.DrawLine(avg + normal, avg + normal * 0.5f + tang * 0.5f);
Gizmos.DrawLine(avg + normal, avg + normal * 0.5f - tang * 0.5f);
}
}
}
gizmoDrawing = false;
}
public GameObject CreateTerrain(GameObject root,
ulong numSampleX, float sizeX, float centerX, ulong numSampleY, float sizeY, float centerY,
float[,] heights, bool recomputeNormal = true)
{
// Note that we create terrain with mesh since unity support only square size height map
List <Vector3> vertices = new List <Vector3>();
List <int> indices = new List <int>();
List <Vector2> uvs = new List <Vector2>();
List <Vector3> normals = new List <Vector3>();
float gridSizeX = sizeX / (numSampleX - 1);
float gridSizeY = sizeY / (numSampleY - 1);
float gridStartX = centerX - sizeX * 0.5f;
float gridStartY = centerY - sizeY * 0.5f;
// vertices
for (ulong i = 0; i < numSampleY - 1; i++)
{
for (ulong j = 0; j < numSampleX - 1; j++)
{
// (x, y) = (j, i)
vertices.Add(ConvertRs2Unity(
gridStartX + j * gridSizeX,
gridStartY + i * gridSizeY,
heights[i, j]
));
// (x, y) = (j+1, i+1)
vertices.Add(ConvertRs2Unity(
gridStartX + (j + 1) * gridSizeX,
gridStartY + (i + 1) * gridSizeY,
heights[i + 1, j + 1]
));
// (x, y) = (j+1, i)
vertices.Add(ConvertRs2Unity(
gridStartX + (j + 1) * gridSizeX,
gridStartY + i * gridSizeY,
heights[i, j + 1]
));
// (x, y) = (j, i)
vertices.Add(ConvertRs2Unity(
gridStartX + j * gridSizeX,
gridStartY + i * gridSizeY,
heights[i, j]
));
// (x, y) = (j, i+1)
vertices.Add(ConvertRs2Unity(
gridStartX + j * gridSizeX,
gridStartY + (i + 1) * gridSizeY,
heights[i + 1, j]
));
// (x, y) = (j+1, i+1)
vertices.Add(ConvertRs2Unity(
gridStartX + (j + 1) * gridSizeX,
gridStartY + (i + 1) * gridSizeY,
heights[i + 1, j + 1]
));
}
}
// triangles
for (int i = 0; i < vertices.Count; i++)
{
indices.Add(i);
}
if (!recomputeNormal)
{
// normals
for (int i = 0; i < vertices.Count; i += 3)
{
Vector3 point1 = ConvertUnity2Rs(vertices[i].x, vertices[i].y, vertices[i].z);
Vector3 point2 = ConvertUnity2Rs(vertices[i + 1].x, vertices[i + 1].y, vertices[i + 1].z);
Vector3 point3 = ConvertUnity2Rs(vertices[i + 2].x, vertices[i + 2].y, vertices[i + 2].z);
Vector3 diff1 = point2 - point1;
Vector3 diff2 = point3 - point2;
Vector3 norm = Vector3.Cross(diff1, diff2);
norm = Vector3.Normalize(norm);
norm = ConvertRs2Unity(norm.x, norm.y, norm.z);
normals.Add(norm);
normals.Add(norm);
normals.Add(norm);
}
}
// uvs
for (ulong i = 0; i < numSampleY - 1; i++)
{
for (ulong j = 0; j < numSampleX - 1; j++)
{
// (x, y) = (j, i)
uvs.Add(new Vector2(
(j / (float)numSampleX),
(i / (float)numSampleY)
));
// (x, y) = (j+1, i+1)
uvs.Add(new Vector2(
((j + 1) / (float)numSampleX),
((i + 1) / (float)numSampleY)
));
// (x, y) = (j+1, i)
uvs.Add(new Vector2(
((j + 1) / (float)numSampleX),
(i / (float)numSampleY)
));
// (x, y) = (j, i)
uvs.Add(new Vector2(
(j / (float)numSampleX),
(i / (float)numSampleY)
));
// (x, y) = (j, i+1)
uvs.Add(new Vector2(
(j / (float)numSampleX),
((i + 1) / (float)numSampleY)
));
// (x, y) = (j+1, i+1)
uvs.Add(new Vector2(
((j + 1) / (float)numSampleX),
((i + 1) / (float)numSampleY)
));
}
}
var terrain = new GameObject("terrain");
terrain.transform.SetParent(root.transform, true);
terrain.AddComponent <MeshFilter>();
terrain.AddComponent <MeshRenderer>();
terrain.GetComponent <MeshFilter>().mesh = new Mesh();
terrain.GetComponent <MeshFilter>().mesh.indexFormat = IndexFormat.UInt32;
terrain.GetComponent <MeshFilter>().mesh.vertices = vertices.ToArray();
terrain.GetComponent <MeshFilter>().mesh.triangles = indices.ToArray();
terrain.GetComponent <MeshFilter>().mesh.uv = uvs.ToArray();
if (!recomputeNormal)
{
terrain.GetComponent <MeshFilter>().mesh.normals = normals.ToArray();
}
else
{
terrain.GetComponent <MeshFilter>().mesh.RecalculateNormals();
}
return(terrain);
}