Tanoda
Movement.cs
Go to the documentation of this file.
1/******************************************************************************
2 * Copyright (C) Ultraleap, Inc. 2011-2020. *
3 * *
4 * Use subject to the terms of the Apache License 2.0 available at *
5 * http://www.apache.org/licenses/LICENSE-2.0, or another agreement *
6 * between Ultraleap and you, your company or other organization. *
7 ******************************************************************************/
8
9using UnityEngine;
10
11namespace Leap.Unity {
12
13 public struct Movement {
14
18 public Vector3 velocity;
19
24 public Vector3 angularVelocity;
25
26 public static readonly Movement identity = new Movement();
27
29 get { return new Movement(-velocity, -angularVelocity); }
30 }
31
32 public static Movement operator *(Movement movement, float multiplier) {
33 return new Movement(movement.velocity * multiplier,
34 movement.angularVelocity * multiplier);
35 }
36
37 public static Movement operator /(Movement movement, float divisor) {
38 return movement * (1f / divisor);
39 }
40
41 public Pose ToPose() {
42 var angVelMag = angularVelocity.magnitude;
43 return new Pose(velocity,
44 Quaternion.AngleAxis(angVelMag, angularVelocity / angVelMag));
45 }
46
47 public static Movement operator +(Movement movement0, Movement movement1) {
48 return new Movement(movement0.velocity + movement1.velocity,
49 movement0.angularVelocity + movement1.angularVelocity);
50 }
51
55 public Movement(Vector3 velocity) {
56 this.velocity = velocity;
57 this.angularVelocity = Vector3.zero;
58 }
59
63 public Movement(Vector3 velocity, Vector3 angularVelocity) {
64 this.velocity = velocity;
65 this.angularVelocity = angularVelocity;
66 }
67
73 public Movement(Pose fromPose, Pose toPose, float dt = 1f) {
74 Vector3 deltaPosition = toPose.position - fromPose.position;
75 Quaternion deltaRotation = Quaternion.Inverse(fromPose.rotation) * toPose.rotation;
76
77 this.velocity = deltaPosition / dt;
78 this.angularVelocity = deltaRotation.ToAngleAxisVector() / dt;
79 }
80
81 #region Accelerations
82
87 public void Integrate(Vector3 linearAcceleration,
88 float deltaTime) {
89 velocity += linearAcceleration * deltaTime;
90 }
91
96 public void Integrate(Vector3 linearAcceleration,
97 Vector3 angularAcceleration,
98 float deltaTime) {
99 velocity += linearAcceleration * deltaTime;
100 angularVelocity += angularAcceleration * deltaTime;
101 }
102
103 #endregion
104
105 }
106
107 public struct KinematicState {
108
109 public Pose pose;
111
113 this.pose = pose;
114 this.movement = movement;
115 }
116
117 public void Integrate(float deltaTime) {
118 pose = pose.Integrated(movement, deltaTime);
119 }
120
121 public void Integrate(Vector3 linearAcceleration,
122 float deltaTime) {
123 movement.Integrate(linearAcceleration, deltaTime);
124 pose = pose.Integrated(movement, deltaTime);
125 }
126
127 public void Integrate(Vector3 linearAcceleration,
128 Vector3 angularAcceleration,
129 float deltaTime) {
130 movement.Integrate(linearAcceleration, angularAcceleration, deltaTime);
131 pose = pose.Integrated(movement, deltaTime);
132 }
133
134 }
135
136 public static class MovementExtensions {
137
138 public static Pose Integrated(this Pose thisPose, Movement movement, float deltaTime) {
139 thisPose.position = movement.velocity * deltaTime + thisPose.position;
140
141 if (movement.angularVelocity.sqrMagnitude > 0.00001f) {
142 var angVelMag = movement.angularVelocity.magnitude;
143 thisPose.rotation = Quaternion.AngleAxis(angVelMag * deltaTime,
144 movement.angularVelocity / angVelMag)
145 * thisPose.rotation;
146 }
147
148 return thisPose;
149 }
150
151 }
152
153}
void Integrate(float deltaTime)
Definition: Movement.cs:117
KinematicState(Pose pose, Movement movement)
Definition: Movement.cs:112
void Integrate(Vector3 linearAcceleration, float deltaTime)
Definition: Movement.cs:121
void Integrate(Vector3 linearAcceleration, Vector3 angularAcceleration, float deltaTime)
Definition: Movement.cs:127
Movement(Pose fromPose, Pose toPose, float dt=1f)
Returns the Movement necessary to go from Pose p0 to Pose p1 in dt seconds. You can ignore the time p...
Definition: Movement.cs:73
static Movement operator/(Movement movement, float divisor)
Definition: Movement.cs:37
static Movement operator*(Movement movement, float multiplier)
Definition: Movement.cs:32
static Movement operator+(Movement movement0, Movement movement1)
Definition: Movement.cs:47
Movement(Vector3 velocity)
Constructs a linear Movement involving no rotation.
Definition: Movement.cs:55
Vector3 angularVelocity
Angular velocity expressed as an angle-axis vector with angle equal to the length of the vector in de...
Definition: Movement.cs:24
Movement inverse
Definition: Movement.cs:28
void Integrate(Vector3 linearAcceleration, float deltaTime)
Discretely integrates this Movement's velocity by a linear acceleration over deltaTime.
Definition: Movement.cs:87
void Integrate(Vector3 linearAcceleration, Vector3 angularAcceleration, float deltaTime)
Discretely integrates this Movement's velocity and angular velocity by both a linear acceleration ter...
Definition: Movement.cs:96
static readonly Movement identity
Definition: Movement.cs:26
Vector3 velocity
The linear velocity of this Movement.
Definition: Movement.cs:18
Movement(Vector3 velocity, Vector3 angularVelocity)
Constructs a Movement with a specified linear velocity and an angular velocity.
Definition: Movement.cs:63
A position and rotation. You can multiply two poses; this acts like Matrix4x4 multiplication,...
Definition: Pose.cs:21
Quaternion rotation
Definition: Pose.cs:24
Vector3 position
Definition: Pose.cs:23