Skip to content

Commit ac90713

Browse files
Sylvain Bertrandrjgriffin42
Sylvain Bertrand
authored and
rjgriffin42
committed
Added extraction of the tuple's normal/tangential part given a normal.
1 parent 2e39ec5 commit ac90713

File tree

1 file changed

+204
-0
lines changed

1 file changed

+204
-0
lines changed

‎ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/EuclidCoreMissingTools.java‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,18 @@
99
import us.ihmc.euclid.matrix.RotationMatrix;
1010
import us.ihmc.euclid.matrix.interfaces.CommonMatrix3DBasics;
1111
import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics;
12+
import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException;
13+
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameTuple3DBasics;
14+
import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DBasics;
15+
import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly;
16+
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
17+
import us.ihmc.euclid.tools.EuclidCoreTools;
18+
import us.ihmc.euclid.tools.TupleTools;
1219
import us.ihmc.euclid.tuple2D.Point2D;
1320
import us.ihmc.euclid.tuple2D.interfaces.Point2DBasics;
1421
import us.ihmc.euclid.tuple2D.interfaces.Point2DReadOnly;
1522
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DBasics;
23+
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
1624
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
1725
import us.ihmc.euclid.tuple4D.interfaces.QuaternionBasics;
1826
import us.ihmc.euclid.tuple4D.interfaces.QuaternionReadOnly;
@@ -481,4 +489,200 @@ else if (percentage < 0.0)
481489
double dy = projectionY - pointY;
482490
return dx * dx + dy * dy;
483491
}
492+
493+
/**
494+
* Calculates the 3D part of the given {@code input} that is parallel to the given
495+
* {@code normalAxis} and stores the result in {@code inputNormalPartToPack}.
496+
* <p>
497+
* The result has the following properties:
498+
* <ul>
499+
* <li><tt>x.n = x<sub>n</sub>.n</tt>
500+
* <li><tt>|x<sub>n</sub>&times;n| = 0</tt>
501+
* </ul>
502+
* where:
503+
* <ul>
504+
* <li><tt>x</tt> is {@code input}.
505+
* <li></tt>n</tt> is {@code normalAxis}.
506+
* <li><tt>x<sub>n</sub></tt> is {@code inputNormalPartToPack}.
507+
* </ul>
508+
* </p>
509+
*
510+
* @param input the tuple to extract the normal part of. Not modified.
511+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
512+
* modified.
513+
* @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified.
514+
*/
515+
public static void extractNormalPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputNormalPartToPack)
516+
{
517+
double normalX = normalAxis.getX();
518+
double normalY = normalAxis.getY();
519+
double normalZ = normalAxis.getZ();
520+
double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ);
521+
522+
double dot = TupleTools.dot(normalAxis, input) / normalLengthSquared;
523+
inputNormalPartToPack.setAndScale(dot, normalAxis);
524+
}
525+
526+
/**
527+
* Calculates the 3D part of the given {@code input} that is parallel to the given
528+
* {@code normalAxis} and stores the result in {@code inputNormalPartToPack}.
529+
* <p>
530+
* The result has the following properties:
531+
* <ul>
532+
* <li><tt>x.n = x<sub>n</sub>.n</tt>
533+
* <li><tt>|x<sub>n</sub>&times;n| = 0</tt>
534+
* </ul>
535+
* where:
536+
* <ul>
537+
* <li><tt>x</tt> is {@code input}.
538+
* <li></tt>n</tt> is {@code normalAxis}.
539+
* <li><tt>x<sub>n</sub></tt> is {@code inputNormalPartToPack}.
540+
* </ul>
541+
* </p>
542+
*
543+
* @param input the tuple to extract the normal part of. Not modified.
544+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
545+
* modified.
546+
* @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified.
547+
* @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference
548+
* frame.
549+
*/
550+
public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputNormalPartToPack)
551+
{
552+
input.checkReferenceFrameMatch(normalAxis, inputNormalPartToPack);
553+
extractNormalPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputNormalPartToPack);
554+
}
555+
556+
/**
557+
* Calculates the 3D part of the given {@code input} that is parallel to the given
558+
* {@code normalAxis} and stores the result in {@code inputNormalPartToPack}.
559+
* <p>
560+
* The result has the following properties:
561+
* <ul>
562+
* <li><tt>x.n = x<sub>n</sub>.n</tt>
563+
* <li><tt>|x<sub>n</sub>&times;n| = 0</tt>
564+
* </ul>
565+
* where:
566+
* <ul>
567+
* <li><tt>x</tt> is {@code input}.
568+
* <li></tt>n</tt> is {@code normalAxis}.
569+
* <li><tt>x<sub>n</sub></tt> is {@code inputNormalPartToPack}.
570+
* </ul>
571+
* </p>
572+
*
573+
* @param input the tuple to extract the normal part of. Not modified.
574+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
575+
* modified.
576+
* @param inputNormalPartToPack the tuple used to store the normal part of the input. Modified.
577+
* @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed
578+
* in the same reference frame.
579+
*/
580+
public static void extractNormalPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputNormalPartToPack)
581+
{
582+
input.checkReferenceFrameMatch(normalAxis);
583+
inputNormalPartToPack.setReferenceFrame(input.getReferenceFrame());
584+
extractNormalPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputNormalPartToPack);
585+
}
586+
587+
/**
588+
* Calculates the 3D part of the given {@code input} that is orthogonal to the given
589+
* {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}.
590+
* <p>
591+
* The result has the following properties:
592+
* <ul>
593+
* <li><tt>x<sub>t</sub>.n = 0</tt>
594+
* <li><tt>|x - (x.n)n| = |x<sub>t</sub>|</tt>
595+
* </ul>
596+
* where:
597+
* <ul>
598+
* <li><tt>x</tt> is {@code input}.
599+
* <li></tt>n</tt> is {@code normalAxis}.
600+
* <li><tt>x<sub>t</sub></tt> is {@code inputTangentialPartToPack}.
601+
* </ul>
602+
* </p>
603+
*
604+
* @param input the tuple to extract the tangential part of. Not modified.
605+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
606+
* modified.
607+
* @param inputTangentialPartToPack the tuple used to store the tangential part of the input.
608+
* Modified.
609+
*/
610+
public static void extractTangentialPart(Tuple3DReadOnly input, Vector3DReadOnly normalAxis, Tuple3DBasics inputTagentialPartToPack)
611+
{
612+
double normalX = normalAxis.getX();
613+
double normalY = normalAxis.getY();
614+
double normalZ = normalAxis.getZ();
615+
double normalLengthSquared = EuclidCoreTools.normSquared(normalX, normalY, normalZ);
616+
617+
double dot = TupleTools.dot(normalX, normalY, normalZ, input) / normalLengthSquared;
618+
double normalPartX = dot * normalX;
619+
double normalPartY = dot * normalY;
620+
double normalPartZ = dot * normalZ;
621+
622+
inputTagentialPartToPack.set(input);
623+
inputTagentialPartToPack.sub(normalPartX, normalPartY, normalPartZ);
624+
}
625+
626+
/**
627+
* Calculates the 3D part of the given {@code input} that is orthogonal to the given
628+
* {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}.
629+
* <p>
630+
* The result has the following properties:
631+
* <ul>
632+
* <li><tt>x<sub>t</sub>.n = 0</tt>
633+
* <li><tt>|x - (x.n)n| = |x<sub>t</sub>|</tt>
634+
* </ul>
635+
* where:
636+
* <ul>
637+
* <li><tt>x</tt> is {@code input}.
638+
* <li></tt>n</tt> is {@code normalAxis}.
639+
* <li><tt>x<sub>t</sub></tt> is {@code inputTangentialPartToPack}.
640+
* </ul>
641+
* </p>
642+
*
643+
* @param input the tuple to extract the tangential part of. Not modified.
644+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
645+
* modified.
646+
* @param inputTangentialPartToPack the tuple used to store the tangential part of the input.
647+
* Modified.
648+
* @throws ReferenceFrameMismatchException if the arguments are not expressed in the same reference
649+
* frame.
650+
*/
651+
public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FixedFrameTuple3DBasics inputTangentialPartToPack)
652+
{
653+
input.checkReferenceFrameMatch(normalAxis, inputTangentialPartToPack);
654+
extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack);
655+
}
656+
657+
/**
658+
* Calculates the 3D part of the given {@code input} that is orthogonal to the given
659+
* {@code normalAxis} and stores the result in {@code inputTangentialPartToPack}.
660+
* <p>
661+
* The result has the following properties:
662+
* <ul>
663+
* <li><tt>x<sub>t</sub>.n = 0</tt>
664+
* <li><tt>|x - (x.n)n| = |x<sub>t</sub>|</tt>
665+
* </ul>
666+
* where:
667+
* <ul>
668+
* <li><tt>x</tt> is {@code input}.
669+
* <li></tt>n</tt> is {@code normalAxis}.
670+
* <li><tt>x<sub>t</sub></tt> is {@code inputTangentialPartToPack}.
671+
* </ul>
672+
* </p>
673+
*
674+
* @param input the tuple to extract the tangential part of. Not modified.
675+
* @param normalAxis the normal vector. It is normalized internally if needed. Not
676+
* modified.
677+
* @param inputTangentialPartToPack the tuple used to store the tangential part of the input.
678+
* Modified.
679+
* @throws ReferenceFrameMismatchException if {@code input} and {@code normalAxis} are not expressed
680+
* in the same reference frame.
681+
*/
682+
public static void extractTangentialPart(FrameTuple3DReadOnly input, FrameVector3DReadOnly normalAxis, FrameTuple3DBasics inputTangentialPartToPack)
683+
{
684+
input.checkReferenceFrameMatch(normalAxis);
685+
inputTangentialPartToPack.setReferenceFrame(input.getReferenceFrame());
686+
extractTangentialPart((Tuple3DReadOnly) input, (Vector3DReadOnly) normalAxis, (Tuple3DBasics) inputTangentialPartToPack);
687+
}
484688
}

0 commit comments

Comments
 (0)